"""
基于有限状态机的白方防御策略
实现分层状态机：全局策略状态 + 单位个体状态

作者：何逸凡
版本：2.1 - FSM策略实现

函数组织结构：
1. 全局状态管理函数
2. DEPLOYMENT阶段函数
3. PATROL阶段函数  
4. DEFENSIVE阶段函数
5. 通用工具函数
6. 无人机相关函数
"""


import numpy as np
import math
import sys
sys.path.insert(0, sys.path[0]+"/../")


from typing import List, Optional, Tuple, Dict, Any, cast, Sequence, Set
from enum import Enum, auto
import scipy.optimize as opt
from sklearn.cluster import KMeans
from scipy.optimize import linear_sum_assignment

from agents.base_agent import RuleBasedAgent, AgentAction, AgentObservation
# Removed unused imports from dynamic_uav_planner
from core.units import WhiteBoat, WhiteDrone, BlackBoat
from core.config import GameConfig
from core.utils import POLYGON_POINTS, is_in_battle_area, point_to_line_segment_nearest_point
from algorithm.tsp_solver import solve_tsp_in_memory  # 新增：导入TSP内存解算器
from algorithm.predator_prey_capture import compute_capture  # 新增：导入捕食者-猎物拦截算法
 

class GlobalState(Enum):
    """全局策略状态"""
    DEPLOYMENT = auto()      # 部署阶段：建立防线
    PATROL = auto()         # 巡航阶段：前往拦截
    DEFENSIVE = auto()      # 防守阶段：敌人接近防线


class TaskAssignmentManager:
    """任务分配管理器 - 负责白方无人船的目标分配"""
    
    def __init__(self):
        """初始化任务分配管理器"""
        self.last_assignment = None
    
    
    def is_ready(self) -> bool:
        """检查任务分配器是否准备就绪"""
        return True
    
    def assign_targets_to_boats(self, targets: List[Dict], boats) -> Dict[int, List[Dict]]:
        """
        为白方无人船分配目标
        
        Args:
            targets: 目标信息列表 [{'id': tid, 'position': (x, y), ...}, ...]
            boats: 白方无人船列表（任何具有id, x, y属性的对象）
            
        Returns:
            Dict[boat_id, assigned_targets]: 分配结果
        """
        if not self.is_ready():
            print("[TaskAssignment] 任务分配器未准备就绪")
            return {}
        
        if not targets or not boats:
            print("[TaskAssignment] 目标或无人船列表为空")
            return {}
        
        try:
            # 提取位置
            target_positions = np.array([target['position'] for target in targets])
            boat_positions = np.array([[boat.x, boat.y] for boat in boats])
            boat_ids = [boat.id for boat in boats]
            
            print(f"[TaskAssignment] 开始分配任务: {len(targets)}个目标, {len(boats)}艘无人船")
            
            # 动态计算集群数量：min(目标数, 无人船数)
            num_clusters = min(len(targets), len(boats))
            if num_clusters == 0:
                return {}
            
            print(f"[TaskAssignment] 使用 {num_clusters} 个集群进行聚类")
            
            # 执行 K-Means 聚类（假设使用 y 轴或其他方式，这里简化使用标准 KMeans）
            kmeans = KMeans(n_clusters=num_clusters, random_state=42, n_init=10)
            labels = kmeans.fit_predict(target_positions)
            centers = kmeans.cluster_centers_
            
            # 构建组：每个簇的目标
            groups = [[] for _ in range(num_clusters)]
            for idx, label in enumerate(labels):
                groups[label].append(targets[idx])
            
            # 构建代价矩阵：boat 到 center 的距离
            cost_matrix = np.zeros((len(boats), len(centers)))
            for i, boat_pos in enumerate(boat_positions):
                for j, center in enumerate(centers):
                    cost_matrix[i, j] = np.linalg.norm(boat_pos - center)
            
            # 使用匈牙利算法求解最优分配
            row_ind, col_ind = opt.linear_sum_assignment(cost_matrix)
            
            # 生成分配结果
            assignment_result = {}
            assigned_clusters = set()
            for r, c in zip(row_ind, col_ind):
                if c < len(centers):  # 安全检查
                    boat_id = boat_ids[r]
                    assignment_result[boat_id] = groups[c]
                    assigned_clusters.add(c)
            
            # 处理未分配的 boats（如果 boats > clusters）
            unassigned_boats = [bid for bid in boat_ids if bid not in assignment_result]
            if unassigned_boats:
                print(f"[TaskAssignment] {len(unassigned_boats)} 艘船未分配任务（可作为预备队）")
            
            # 处理未分配的 clusters（如果 clusters > boats）
            unassigned_count = len(centers) - len(assigned_clusters)
            if unassigned_count > 0:
                print(f"[TaskAssignment] {unassigned_count} 个目标簇未分配（资源不足）")
            
            # 计算总代价
            total_cost = cost_matrix[row_ind, col_ind].sum()
            print(f"[TaskAssignment] 任务分配完成，总代价: {total_cost:.2f}")
            
            # 记录分配历史
            self.last_assignment = {
                'targets': targets,
                'boats': boat_ids,
                'result': assignment_result,
                'cost': total_cost,
                'timestamp': None  # 将在调用处设置
            }
            
            return assignment_result
            
        except Exception as e:
            print(f"[TaskAssignment] 任务分配失败: {e}")
            return {}
    


    # Removed unused logging/statistics helpers: _log_assignment_details, get_assignment_statistics


class FSMDefenseAgent(RuleBasedAgent):
    """
    基于有限状态机的白方防御策略
    
    采用全局状态机设计：
    - 全局状态机：控制整体策略和资源分配
    - 直接基于全局状态决策，无需复杂的个体状态管理
    """
    
    def __init__(self):
        super().__init__("FSMDefenseAgent", "defensive")
        
        # 全局状态
        self.global_state = GlobalState.DEPLOYMENT
        self.state_timer = 0.0
        
        # 状态数据
        self.boat_targets = {}    # {boat_id: target_position}
        self.drone_targets = {}   # {drone_id: target_info}
        self.detected_targets = {}  # {target_id: enhanced_target_data}
        
        # 船只拦截点信息 - 专门存储DEPLOYMENT阶段的拦截点，避免与目标分配冲突
        self.boat_intercept_points: Dict[int, Tuple[float, float]] = {}
        
        # 任务分配管理器
        self.task_manager = TaskAssignmentManager()
        
        # DEPLOYMENT 阶段参数
        self.deployment_line_x = 60000
        self.deployment_line_y_start = 61655      # 线段起始y坐标
        self.deployment_line_y_end = 611550      # 线段结束y坐标
        
        # 封锁线坐标（A1A6线）
        self.blockade_line_x = 50000

        # 无人机任务状态
        self.drone_mission_state = {}  # {drone_id: mission_info}
        self.drone_waypoints = self._init_drone_waypoints()
        
        # 黑方目标分组 (TODO: 待实现)
        self.target_groups = {}  # {group_id: [target_ids]}
        self.boat_group_assignments = {}  # {boat_id: group_id}
        
        # 巡航无人机执行顺序状态
        self.patrol_drone_sequence = "drone_1_active"  # 可选值："drone_1_active", "drone_3_active"
        
        # 1号机返航状态
        self.drone1_returning = False
        
        # 分离的TSP索引，分别用于1号机和3号机
        self._drone1_tsp_index = 0
        self._drone3_tsp_index = 0
        
        # 独立的TSP路径，分别用于1号机和3号机
        self._drone1_tsp_path = None
        self._drone3_tsp_path = None
        
        # 新增：记录每艘无人船的拦截信息
        self.boat_intercept_info = {}  # {boat_id: {'target_id': int, 'intercept_point': (x, y), 'intercept_time': float, 'scan_index': int}}
        
        # 拦截点缓存机制 - 避免在目标未探测到时拦截点变化
        self.cached_intercept_points = {}  # {boat_id: {'target_id': str, 'intercept_point': (x, y), 'last_update': float}}
        
        # DEFENSIVE阶段无人机警戒扫视状态
        self.drone_alert_state = {}  # {drone_id: {'state': 'charging'|'alert_scanning'|'monitoring', 'last_alert_time': float, 'target_id': str|None, 'scan_center': (x,y), 'scan_angle': float}}
        
        # 新增：无人机发现的近距离目标通知
        self.drone_close_target_alerts = {}  # {boat_id: {'target_id': str, 'alert_time': float, 'handled': bool}}
        
        # 新增：目标击沉后的通知机制
        self.target_destruction_notifications = []  # [(target_id, original_assigned_boat_id, destruction_time), ...]
        
        # 调试计数器
        # 调试计数器
        self._target_update_debug_count = 0

        # 2号无人机TSP路径相关
        self._drone2_tsp_path = None
        self._drone2_tsp_index = 0

        # PATROL滚动再分配/重规划参数
        self.patrol_reassign_interval: float = 4000.0  # 用户指定：8000秒
        self.patrol_last_reassign_time: float = 0.0
        self.patrol_last_tsp_recompute_time: float = 0.0
        self.patrol_phase_start_time: float = 0.0
        self.drone1_launch_time: Optional[float] = None
        self.drone3_started: bool = False
        # 无人机应急调度（换位降落/代飞）
        self.drone_swap_tasks: Dict[int, Dict] = {}
        self.drone_emergency_target_boat: Dict[int, int] = {}

        # ========== DEFENSIVE阶段参数 ==========
        self.defense_reassign_interval: float = 120.0
        self._defense_last_reassign_time: float = 0.0
        # 锁定环控制目标半径区间（米）
        self.lock_ring_radius: Tuple[float, float] = (35000.0, 38000.0)
        # 白艇最小安全间距（米）
        self.min_boat_separation: float = 5000.0
        # 交战计划记录
        self.boat_engagement_plan: Dict[int, Dict] = {}
        # 3号机PATROL完整盘旋状态
        self._patrol_drone3_circle: Dict[str, Any] = {
            'active': False,
            'center': None,
            'last_angle': None,
            'accum_angle': 0.0,
            'radius_m': 300.0,
        }
        # 当DEFENSIVE重分配拟切换队首目标时，需要至少提升该船ETA阈值（5%）才切换，避免抖动
        self.defense_eta_switch_threshold: float = 0.05

        # 一次性盘旋尝试记忆（按 target_id 字符串）
        self.drone_extra_circle_done: Set[str] = set()

        # 新增：用于存储防御阶段无任务船只的巡逻状态
        self.boat_patrol_circles = {}

        # DEFENSIVE阶段：部署线巡逻中心与参数
        self._defensive_patrol_assignments: Dict[int, Tuple[float, float]] = {}
        self.idle_patrol_radius_m: float = 2000.0
        self.idle_patrol_speed_ratio: float = 0.8

        # 白-白安全间距（仅DEFENSIVE阶段启用）与事件记录
        self.separation_preventive_threshold: float = 250.0  # 预防阈值
        self.separation_collision_threshold: float = 200.0   # 硬阈值（不得进入）
        self.separation_release_threshold: float = 240.0     # 强制避让释放阈值
        self.separation_events: List[Dict[str, Any]] = []
        self._separation_force_boats: Set[int] = set()

        # 新增：已销毁目标ID集合，防止重复处理
        self.destroyed_target_ids: Set[str] = set()

    def _init_drone_waypoints(self) -> Dict[int, List[Tuple[float, float]]]:
        """
        初始化无人机的航点任务
        0、2、4号无人机分配指定航点，1、3号无人机航点为空
        """
        
        # return {
        #     0: [(244000, -280000+336302), (264000, -208000+336302)],  # 0号无人机（最下面）
        #     1: [(265000, -48000+336302), (265000, -139000+336302)],   # 1号无人机
        #     2: [(244000, 336302), (264000, 336302)],        # 2号无人机
        #     3: [(265000, 48000+336302), (265000, 139000+336302)], # 3号无人机
        #     4: [(244000, 280000+336302), (264000, 208000+336302)] # 4号无人机（最上面）
        # }

        drone_waypoints = {
            0: [
                (248837, +336302-281343),
                (258435, +336302-212128),
                (259530, +336302-137195)
            ],
            1: [],
            2: [
                (259261, 72971+336302),
                (264510, 1174+336302),
                (261560, -68772+336302)
            ],
            3: [],
            4: [
                (248837, 281343+336302),
                (260054, 217009+336302),
                (256814, 144642+336302)
            ]
        }
        return drone_waypoints

    def reset(self):
        """重置FSM状态"""
        super().reset()
        self.global_state = GlobalState.DEPLOYMENT
        self.state_timer = 0.0
        self.boat_targets.clear()
        self.drone_targets.clear()
        self.detected_targets.clear()
        self.drone_mission_state.clear()
        self.target_groups.clear()
        self.boat_group_assignments.clear()
        self.cached_intercept_points.clear()
        self.drone_alert_state.clear()
        self.drone_close_target_alerts.clear()
        self.target_destruction_notifications.clear()

        # 重置2号无人机TSP路径
        self._drone2_tsp_path = None
        self._drone2_tsp_index = 0

        # 清空一次性盘旋尝试记录
        self.drone_extra_circle_done.clear()

        # 新增：清空已销毁目标ID集合
        self.destroyed_target_ids.clear()

        self.interceptable_targets_cache = None
        self.interceptable_targets_cache_time = -1
        
        # 重置防御阶段滚动再分配计时器
        self._defense_last_reassign_time = -1
        
        self.boat_patrol_circles.clear()
        # 清理防御巡逻中心分配
        self._defensive_patrol_assignments = {}

        # 重置任务分配状态
        if hasattr(self, '_final_assignment_done'):
            delattr(self, '_final_assignment_done')
        
        # 重新初始化任务分配管理器
        self.task_manager = TaskAssignmentManager()
    
    def get_actions(self, observation: AgentObservation) -> AgentAction:
        """主要决策函数"""
        current_time = observation.current_time
        
        # 更新状态计时器
        if hasattr(self, '_last_time'):
            self.state_timer += current_time - self._last_time
        self._last_time = current_time
        
        # 更新目标信息
        self._update_detected_targets(observation)
        # 清理越过封锁线的目标（直接视为消失）
        self._cleanup_targets_crossed_blockade()
        # print(observation.get_detected_targets())
        # print(self.detected_targets)
        # 全局状态机更新
        self._update_global_state(observation)
        

        
        # 生成动作
        action = AgentAction()
        
        # 无人艇动作
        active_boats = observation.get_active_boats()
        boat_actions = np.zeros((5, 3))
        for boat in active_boats:
            raw = self._generate_boat_action(boat, observation)
            # 先应用白对白分离安全壳
            separated = self._apply_white_boat_separation(boat, raw, observation)
            # 再应用黑方30km硬保距（优先级高于白对白分离）
            final = self._apply_black_keepout(boat, separated, observation)
            boat_actions[boat.id] = final
        action.boat_actions = boat_actions
        
        # 无人机动作
        # 获取所有无人机（包括停靠在母舰上的无人机）
        all_drones = observation.all_drones
        if all_drones:
            drone_actions = np.zeros((len(all_drones), 3))
            for drone in all_drones:
                drone_actions[drone.id] = self._generate_drone_action(drone, observation)
            action.drone_actions = drone_actions
            # print(action.drone_actions)
        
        # 验证任务分配状态
        # if self.global_state == GlobalState.PATROL and not self.validate_task_assignment():
        #     print("[FSM] 警告：PATROL状态下任务分配无效，尝试重新分配")
        #     self.force_reassign_tasks(observation)


        
        return action

    # ==================== 任务分配函数 ====================
    
    def _assign_tasks(self, observation: AgentObservation):
        """
        进行任务分配
        """
        
        print(f"[FSM] 执行任务分配，时间: {observation.current_time}")
        # 收集可拦截的目标
        interceptable_targets = self._collect_interceptable_targets(observation)
        if not interceptable_targets:
            print("[FSM] 没有可拦截的目标，无法进行任务分配")
            return
        # 获取白方无人船
        white_boats = [boat for boat in observation.white_boats if boat.active]
        if not white_boats:
            print("[FSM] 没有可用的白方无人船")
            return
        # 执行任务分配（优先采用ETA+ΔY代价函数）
        try:
            assignment_result = self._assign_tasks_with_eta(interceptable_targets, white_boats)
        except Exception as e:
            print(f"[FSM] ETA分配失败，回退到默认分配: {e}")
            assignment_result = self.task_manager.assign_targets_to_boats(interceptable_targets, white_boats)
        
        if assignment_result:
            # 应用分配结果
            self._apply_task_assignment(assignment_result, observation.current_time)
            
            
            
    

    
    # ==================== 目标管理函数 ====================
    
    def _update_detected_targets(self, observation: AgentObservation):
        """更新目标信息，使用统一简洁的数据结构"""
        now = observation.current_time
        # 首先更新所有历史目标的预测位置（包括当前未观测到的）
        for target_id, target_data in self.detected_targets.items():
            if target_data['is_frozen'] :
                # 如果目标被冻结，检查冻结时间是否已过
                if  now >= target_data['frozen_until']:
                    # 解冻目标
                    target_data['is_frozen'] = False
                    target_data['frozen_until'] = None
                    print(f"[FSM] 目标{target_id}已解冻")
                else:
                    target_data['speed']= 0.0  # 冻结状态下速度为0
                    target_data['predict_pos'] = target_data['position']  # 保持当前位置不变
            time_diff = now - target_data['last_seen']
            
            # 基于最后观测到的位置、速度、航向预测当前位置
            pred_x = target_data['position'][0] + target_data['speed'] * math.cos(target_data['heading']) * time_diff
            pred_y = target_data['position'][1] + target_data['speed'] * math.sin(target_data['heading']) * time_diff
            target_data['predict_pos'] = (pred_x, pred_y)
        
        # 然后更新当前观测到的目标
        detected_targets = observation.get_detected_targets()
        #print(f"[DEBUG] 观测到的目标数量: {len(detected_targets)}")
        
        for target in detected_targets: 
            target_pos = (target.x, target.y)
            # print(f"[DEBUG] 正在更新目标 {target.id} 到 detected_targets")
            
            # 使用用户要求的数据结构
            # 统一使用字符串键，避免 str/int 键不一致导致的匹配问题
            tid_str = str(target.id)
            self.detected_targets[tid_str] = {
                'id': str(target.id),
                'position': target_pos,
                'speed': target.speed,
                'heading': target.heading,
                'last_seen': now,
                'predict_pos': target_pos,  # 当前观测到的目标，预测位置就是当前位置
                'is_frozen': False,  # 是否被冻结
                'frozen_until': None  # 冻结到的时间
            }

            # 目标再次被探测到时，重置一次性盘旋尝试资格
            if tid_str in self.drone_extra_circle_done:
                self.drone_extra_circle_done.discard(tid_str)
            
       #print(f"[DEBUG] 更新后 detected_targets 包含 {len(self.detected_targets)} 个目标")

    def _cleanup_targets_crossed_blockade(self) -> None:
        """清理越过封锁线(含等于)的目标：从detected_targets与所有boat_targets删除"""
        try:
            to_delete: List[str] = []
            for tid, data in list(self.detected_targets.items()):
                px, py = data.get('predict_pos', data.get('position', (None, None)))
                if not (isinstance(px, (int, float)) and isinstance(py, (int, float))):
                    continue
                if float(px) <= float(self.blockade_line_x):
                    to_delete.append(str(tid))
            if not to_delete:
                return
            for tid in to_delete:
                if tid in self.detected_targets:
                    del self.detected_targets[tid]
                for bid in list(self.boat_targets.keys()):
                    lst = self.boat_targets.get(bid, [])
                    if isinstance(lst, list) and tid in lst:
                        lst.remove(tid)
                print(f"[FSM] 清理：目标 {tid} 已越过封锁线，视为消失并从任务中移除")
        except Exception:
            pass
            

    def _collect_interceptable_targets(self, observation: AgentObservation) -> List[Dict]:
        """收集所有可拦截的目标信息"""
        interceptable_targets = []
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        total_targets = len(self.detected_targets)
        filtered_count = 0
        
        for target_id, target_data in self.detected_targets.items():
            
            # [修正] 使用预测位置进行判断
            if self._can_target_be_intercepted(target_data, white_boats):
                interceptable_targets.append({
                    'id': target_id,
                    'position': target_data.get('predict_pos', target_data['position']),
                    'speed': target_data['speed'],
                    'heading': target_data['heading']
                    
                })
            else:
                filtered_count += 1
                # 添加过滤日志
                print(f"[FSM] 目标{target_id}在位置{target_data['predict_pos']}不可拦截，已从任务分配中过滤")
        
        print(f"[FSM] 拦截目标过滤完成：总目标{total_targets}个，可拦截{len(interceptable_targets)}个，过滤{filtered_count}个")
        return interceptable_targets

    
    def _calculate_intercept_point_with_id(self, boat: WhiteBoat, target_id: str, observation: AgentObservation):

        """根据目标ID计算拦截点"""
        target_data = self.detected_targets.get(str(target_id))
        if not target_data:
            return None
            
        intercept_result = self._calculate_intercept_point(
            boat, target_data.get('predict_pos', target_data['position']), target_data['speed'], target_data['heading']
        )
        if intercept_result is None:
            return None
        intercept_pose, intercept_time, boat_pos = intercept_result
        return intercept_pose

    def _can_target_be_intercepted(self, target_info: Dict, white_boats: List[WhiteBoat]) -> bool:
        """检查目标是否可以被拦截"""
        for boat in white_boats:
            intercept_result = self._calculate_intercept_point(
                boat, target_info.get('predict_pos', target_info['position']), 
                target_info.get('speed', 0.0), 
                target_info.get('heading', 0.0)
            )
            if intercept_result is not None:
                return True
        return False
    
    def _apply_task_assignment(self, assignment_result: Dict[int, List[Dict]], timestamp: float):
        """
        应用任务分配结果
        """
        self.boat_targets.clear()
        
        for boat_id, assigned_targets in assignment_result.items():
            # 优先按ETA排序（如果存在），否则按x值排序
            if assigned_targets and isinstance(assigned_targets[0], dict) and 'eta' in assigned_targets[0]:
                sorted_targets = sorted(assigned_targets, key=lambda t: t.get('eta', float('inf')))
            else:
                sorted_targets = sorted(assigned_targets, key=lambda t: t['position'][0])
            # 提取排序后的 ID 列表
            # 统一将 boat_targets 中的目标ID以字符串形式存储
            self.boat_targets[boat_id] = [str(target['id']) for target in sorted_targets]
            print(f"[FSM] 船只{boat_id}的目标已排序: {self.boat_targets[boat_id]}")
        
        # 更新任务分配时间戳
        if self.task_manager.last_assignment:
            self.task_manager.last_assignment['timestamp'] = timestamp
        
        print(f"[FSM] 任务分配结果已应用到boat_targets（保存目标ID）")
        # 打印分配详情
        for boat_id, target_ids in self.boat_targets.items():
            if target_ids:
                print(f"  船只{boat_id}分配到目标ID: {target_ids}")

    def _estimate_eta(self, boat: WhiteBoat, target_info: Dict) -> float:
        """估计ETA(船→目标)秒数，失败返回inf"""
        try:
            pos = target_info.get('predict_pos', target_info.get('position'))
            speed = float(target_info.get('speed', 0.0))
            heading = float(target_info.get('heading', 0.0))
            inter = self._calculate_intercept_point(boat, pos, speed, heading)
            if inter is None:
                return float('inf')
            _, t, _ = inter
            return float(t)
        except Exception:
            return float('inf')

    def _assign_tasks_with_eta(self, targets: List[Dict], boats: List[WhiteBoat]) -> Dict[int, List[Dict]]:
        """基于加权(ETA, Y差)的任务分配"""
        if not targets or not boats:
            return {}
        
        # 1. 对目标进行聚类
        target_positions = np.array([target.get('predict_pos', target['position']) for target in targets])
        if len(target_positions) < len(boats):
            # 如果目标数少于船数，每个目标自成一类
            labels = np.arange(len(target_positions))
        else:
            kmeans = KMeans(n_clusters=min(len(targets), len(boats)), random_state=42, n_init=10).fit(target_positions)
            labels = kmeans.labels_
        
        # 2. 构建组：每个簇的目标
        groups = [[] for _ in range(min(len(targets), len(boats)))]
        for idx, label in enumerate(labels):
            groups[label].append(targets[idx])
        
        # 3. 构建代价矩阵：boat 到 center 的距离
        alpha, beta = 1.0, 0.3
        cost_matrix = np.zeros((len(boats), len(groups)))
        for i, boat in enumerate(boats):
            for j, group in enumerate(groups):
                if not group:
                    cost_matrix[i, j] = 1e12
                    continue
                etas = [self._estimate_eta(boat, t) for t in group]
                eta_min = min(etas) if etas else float('inf')
                # ΔY采用中位数
                dy_vals = [abs(boat.y - t['position'][1]) for t in group]
                dy_median = float(np.median(dy_vals)) if dy_vals else 0.0
                cost_matrix[i, j] = alpha * (eta_min if np.isfinite(eta_min) else 1e12) + beta * dy_median
        
        # 4. 使用匈牙利算法求解最优分配
        row_ind, col_ind = linear_sum_assignment(cost_matrix)
        
        # 5. 生成分配结果
        new_assignment = {}
        for r, c in zip(row_ind, col_ind):
            if 0 <= c < len(groups):
                boat_id = boats[r].id
                # 为该船的组内目标补充单目标ETA，用于后续排序
                enriched = []
                for t in groups[c]:
                    eta_val = self._estimate_eta(boats[r], t)
                    t2 = dict(t)
                    t2['eta'] = eta_val
                    enriched.append(t2)
                new_assignment[boat_id] = enriched
        
        return new_assignment

    

    def _maybe_reassign_tasks_in_patrol(self, observation: AgentObservation):
        """在PATROL阶段按时间窗口进行滚动再分配与（可选）重算TSP"""
        now = observation.current_time
        if now - self.patrol_last_reassign_time >= getattr(self, 'patrol_reassign_interval', 8000.0):
            print(f"[FSM] PATROL滚动再分配触发，时间: {now}")
            self._assign_tasks(observation)
            self.patrol_last_reassign_time = now
    

    
    def force_reassign_tasks(self, observation: AgentObservation) -> bool:
        """强制重新分配任务（用于动态调整）"""
        print("[FSM] 强制重新分配任务")
        
        # 重置分配状态
        if hasattr(self, '_final_assignment_done'):
            delattr(self, '_final_assignment_done')
        
        # 执行分配
        self._assign_tasks(observation)
        
        return hasattr(self, '_final_assignment_done') 
    
    def validate_task_assignment(self) -> bool:
        """验证当前任务分配的有效性"""
        if not self.boat_targets:
            return False
        
        # 检查是否有船只分配了目标
        has_assigned_targets = any(len(targets) > 0 for targets in self.boat_targets.values())
        
        # 检查任务分配管理器状态
        manager_valid = self.task_manager.is_ready() and self.task_manager.last_assignment is not None
        
        return has_assigned_targets and manager_valid
    

    # 全局状态机更新
    # ==================== 全局状态管理函数 ====================
    
    def _update_global_state(self, observation: AgentObservation):
        """更新全局状态机"""
        current_state = self.global_state

        if current_state == GlobalState.DEPLOYMENT:
            # 部署完成条件：所有无人机完成航点探测任务
            if self._should_enter_PATROL_phase(observation):
                #记录第一阶段时间
                print(f"[FSM] 部署阶段完成，时间: {observation.current_time}")
                print("===进入 PATROL 阶段===")
                self.global_state = GlobalState.PATROL
                self.state_timer = 0.0
                # 执行任务分配
                self._create_drone_patrol_path(observation)
                self._assign_tasks(observation)
                # 记录PATROL开始时间
                self.patrol_phase_start_time = observation.current_time
                self.drone1_launch_time = None
                self.drone3_started = False
                
        elif current_state == GlobalState.PATROL:
            # 检查是否应该进入DEFENSIVE阶段
            if self._should_enter_defensive_phase(observation):
                self.global_state = GlobalState.DEFENSIVE
                self.state_timer = 0.0
                print(f"[FSM] 进入DEFENSIVE阶段，时间: {observation.current_time}")
            else:
                # 滚动再分配
                self._maybe_reassign_tasks_in_patrol(observation)

        elif current_state == GlobalState.DEFENSIVE:
            # DEFENSIVE阶段：敌人接近防线
            # 无人机跟踪和搜索，无人船锁定一切可锁定的目标
            pass
        
    #状态判断
    def _should_enter_PATROL_phase(self, observation: AgentObservation) -> bool:
        """
        检查部署是否完成
        完成条件：所有无人机完成航点探测任务（到达所有航点并完成圆形扫描后返回母舰）
        """
        # 检查每架无人机的任务状态
        for drone_id in [0,1,2,3,4]:  # 5架无人机
            mission_state = self.drone_mission_state.get(drone_id, {})
            
            # 如果无人机的任务还没有完成
            if mission_state.get('phase') != 'mission_completed':
                return False
        
        return True
    
    def _should_enter_defensive_phase(self, observation: AgentObservation) -> bool:
        """
        检查是否应该进入DEFENSIVE阶段
        进入条件：任何黑方无人船的预测位置进入白方无人船45km范围内
        """
        # 条件：检查是否有黑方无人船预测位置在白方无人船45km范围内
        black_boats_in_range = self._check_black_boats_in_45km_range(observation)
        
        if black_boats_in_range:
            print(f"[FSM] DEFENSIVE条件满足：有黑方无人船可能出现在在白方无人船45km范围内")

        return black_boats_in_range

    def _check_black_boats_in_specific_white_45km_range(self, observation: AgentObservation, boat: WhiteBoat) -> List[int]:
        """检查是否有黑方无人船预测位置在指定白方无人船45km范围内
        返回符合条件的黑方无人船ID列表"""
        
        threat_id = []
        for target_id, target_data in self.detected_targets.items():
            pos = target_data['position']
            last_seen = target_data['last_seen']
            now = observation.current_time
            # 检查黑船是否可能在任一白方无人船45km范围内
            
            distance = math.hypot(pos[0] - boat.x, pos[1] - boat.y)-(now - last_seen)*GameConfig.BLACK_BOAT_MAX_SPEED
            if distance <= 45000:
                threat_id.append(target_id)
        return threat_id
    def _check_black_boats_in_45km_range(self, observation: AgentObservation) -> bool:
        """检查是否有黑方无人船预测位置在任一白方无人船45km范围内"""
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        for target_id, target_data in self.detected_targets.items():
            pos = target_data['position']
            last_seen = target_data['last_seen']
            now = observation.current_time
            # 检查黑船是否可能在任一白方无人船45km范围内
            for boat in white_boats:
                distance = math.hypot(pos[0] - boat.x, pos[1] - boat.y)-(now - last_seen)*GameConfig.BLACK_BOAT_MAX_SPEED
                if distance <= 45000:
                    return True
        
        return False

    def _nearest_white_boat(self, boat: WhiteBoat, observation: AgentObservation) -> Optional[WhiteBoat]:
        candidates = [b for b in observation.white_boats if b.active and b.id != boat.id]
        if not candidates:
            return None
        return min(candidates, key=lambda b: math.hypot(b.x - boat.x, b.y - boat.y))

    def _apply_white_boat_separation(self, boat: WhiteBoat, raw_action: np.ndarray, observation: AgentObservation) -> np.ndarray:
        """仅在DEFENSIVE阶段对白-白间距施加安全壳：
        - d < 150m: 预防性避让（减速+切向偏置），记录事件
        - d < 100m 或处于强制避让状态且 d < 120m: 强制径向分离，记录事件
        其它情况返回原动作。
        """
        try:
            if self.global_state != GlobalState.DEFENSIVE:
                return raw_action
            if not boat.active:
                return raw_action
            nearest = self._nearest_white_boat(boat, observation)
            if nearest is None:
                return raw_action
            dx = boat.x - nearest.x
            dy = boat.y - nearest.y
            d = math.hypot(dx, dy)

            # 不处理非数值
            if not (math.isfinite(d)):
                return raw_action

            # 强制避让：进入<100 或 已处于强制状态且距离未释放
            force = False
            if d < self.separation_collision_threshold:
                force = True
                self._separation_force_boats.add(boat.id)
            elif boat.id in self._separation_force_boats:
                if d < self.separation_release_threshold:
                    force = True
                else:
                    self._separation_force_boats.discard(boat.id)

            if force:
                # 直接沿远离最近船的径向方向加速分离
                heading = math.atan2(dy, dx)
                speed = float(boat.max_speed)
                self._log_separation_event(boat.id, nearest.id, d, 'force', raw_action, np.array([speed, heading, raw_action[2]]))
                return np.array([speed, heading, raw_action[2]])

            # 预防性避让：d < 150
            if d < self.separation_preventive_threshold:
                # 以相对连线的切向方向偏置，同时减速到0.5×max
                base_angle = math.atan2(dy, dx)
                tangent = base_angle + math.pi / 2.0
                speed = float(boat.max_speed * 0.8)
                self._log_separation_event(boat.id, nearest.id, d, 'preventive', raw_action, np.array([speed, tangent, raw_action[2]]))
                return np.array([speed, tangent, raw_action[2]])

            return raw_action
        except Exception:
            return raw_action

    def _log_separation_event(self, boat_id: int, other_id: int, distance: float, mode: str, raw: np.ndarray, fixed: np.ndarray) -> None:
        evt = {
            'mode': mode,
            'time': getattr(self, '_last_time', 0.0),
            'boat_id': int(boat_id),
            'other_boat_id': int(other_id),
            'distance': float(distance),
            'raw': [float(raw[0]), float(raw[1]), float(raw[2])],
            'fixed': [float(fixed[0]), float(fixed[1]), float(fixed[2])],
        }
        self.separation_events.append(evt)
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: 间距{mode}避让  boat={boat_id} other={other_id} d={distance:.1f}m")
    
    
    def _apply_black_keepout(self, boat: WhiteBoat, action: np.ndarray, observation: AgentObservation) -> np.ndarray:
        """在DEFENSIVE阶段对每艘白船应用黑方30km硬保距。
        规则：
        - 使用最近的黑船实时坐标（observation.black_boats 中 active 的单位）
        - 绝不允许进入 < 30km：
          * 若已 <30km：径向最大速远离最近黑船
          * 若 >=30km：做一步穿越预测；若下一步将跌破30km，则修正为径向远离
        - 与白对白分离冲突时：优先保距黑方（本函数在分离修正之后调用）
        - 保持 lock_id 不变，允许边撤离边维持锁定
        """
        try:
            if self.global_state != GlobalState.DEFENSIVE:
                return action
            if not boat.active:
                return action
            # 找最近黑船（使用实时位置）
            nearest_black = None
            min_dist = float('inf')
            for target in observation.black_boats:
                if not getattr(target, 'active', False):
                    continue
                d = math.hypot(boat.x - target.x, boat.y - target.y)
                if math.isfinite(d) and d < min_dist:
                    min_dist = d
                    nearest_black = target
            if nearest_black is None or not math.isfinite(min_dist):
                return action

            keepout_radius = float(GameConfig.BLACK_BOAT_DETECT_RANGE)  # 30km
            # 若已侵入：立即径向远离，最大速度
            if min_dist < keepout_radius:
                heading = math.atan2(boat.y - nearest_black.y, boat.x - nearest_black.x)
                speed = float(boat.max_speed)
                return np.array([speed, heading, action[2]])

            # 预测是否会在下一步穿越30km边界
            current_speed = float(max(0.0, min(float(action[0]), float(boat.max_speed))))
            current_heading = float(action[1])
            # 径向指向敌方的单位向量角
            angle_to_enemy = math.atan2(nearest_black.y - boat.y, nearest_black.x - boat.x)
            # 计算当前速度在朝向敌方径向的投影（>0 表示朝向敌方）
            radial_towards = current_speed * math.cos(current_heading - angle_to_enemy)
            dt = float(getattr(GameConfig, 'TIME_STEP', 1.0))
            projected_distance = min_dist - radial_towards * dt
            if radial_towards > 0.0 and projected_distance < keepout_radius:
                # 修正为径向远离方向，速度略高以保证不穿越
                heading = math.atan2(boat.y - nearest_black.y, boat.x - nearest_black.x)
                speed = float(max(current_speed, boat.max_speed * 0.8))
                return np.array([speed, heading, action[2]])

            return action
        except Exception:
            return action

    def _generate_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """根据全局状态生成单艘无人艇动作"""
        # 检查是否被冻结
        if boat.is_frozen(observation.current_time):
            return np.zeros(3)
        
        # 根据全局状态决策
        if self.global_state == GlobalState.DEPLOYMENT:
            return self._execute_deployment_boat_action(boat, observation)
        elif self.global_state == GlobalState.PATROL:
            return self._execute_patrol_boat_action(boat, observation)
        elif self.global_state == GlobalState.DEFENSIVE:
            return self._execute_defensive_boat_action(boat, observation)
        else:
            return np.zeros(3)

    # ==================== PATROL阶段函数 ====================
    
    def _execute_patrol_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行PATROL阶段的无人船行为"""
        # 获取分配的目标ID列表
        assigned_target_ids = self.boat_targets.get(boat.id)
        
        if not assigned_target_ids:
            #print(f"[FSM] Boat {boat.id} 在PATROL阶段未分配目标，保持位置")# todo 如果patrol阶段没有分配目标,应该有支援的操作
            return self._boat_hold_position(boat, observation)
        
        # 选择第一个分配的目标进行拦截
        target_id = assigned_target_ids[0]
        
        # 计算拦截点
        intercept_point = self._calculate_intercept_point_with_id(boat, target_id, observation)
        
        if intercept_point is not None:
            intercept_x, intercept_y = intercept_point
            dx = intercept_x - boat.x
            dy = intercept_y - boat.y
            distance = math.hypot(dx, dy)
            heading = math.atan2(dy, dx)
            speed =boat.max_speed
            
        else:
            # 如果无法计算拦截点，直接选择下一个目标
            speed = 0.0
            heading = 0.0
            # self.boat_SOS(boat, observation)
            assigned_target_ids.pop(0)
            print(f"[FSM] PATROL阶段:Boat {boat.id} 无法计算目标{target_id}的拦截点，计算下一个目标")

        # PATROL阶段不进行锁定，lock_id = -1
        return np.array([speed, heading, -1])
    
    def _execute_patrol_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """执行PATROL阶段的探测无人机行为：到TSP预测点最小转弯半径全速环扫一圈，并能量不够时执行最近白艇换位返航"""
        current_time = observation.current_time
        # 记录1号机首次起飞时间
        if drone.id == 1 and self.drone1_launch_time is None and drone.docked_on_boat_id == -1:
            self.drone1_launch_time = current_time

        # 能量安全检查：若不足以返母舰，返回最近白艇，并安排最近白艇上的无人机代替回母舰
        safe_action = self._ensure_safe_return_or_swap(drone, observation)
        if safe_action is not None:
            return safe_action
        # 1号机执行逻辑
        if drone.id == 1:
            # 1号机正在返航
            if self.drone1_returning:
                # 检查是否已降落
                if drone.docked_on_boat_id != -1:
                    self.drone1_returning = False
                    self.patrol_drone_sequence = "drone_3_active"
                    print(f"[FSM] 1号无人机已降落，3号无人机开始执行任务")
                    return np.array([0.0, drone.heading, 0.1])
                else:
                    # 继续返航
                    return self._drone_return_to_parent_action(drone, observation)
            
            # 1号机执行任务
            if self.patrol_drone_sequence == "drone_1_active":
                # 确保1号机TSP路径已生成
                if self._drone1_tsp_path is None:
                    print(f"[FSM] 1号无人机TSP路径未生成，重新生成")
                    self._create_drone_patrol_path(observation)
                    self._drone1_tsp_path = self._drone2_tsp_path.copy() if self._drone2_tsp_path else []
                    self._drone1_tsp_index = 0
                
                # 动态移除已探测目标（统一为字符串ID以匹配TSP路径中的字符串ID）
                detected_ids = set(str(t.id) for t in observation.get_single_detected_targets(drone))
                old_path = self._drone1_tsp_path.copy() if self._drone1_tsp_path else []
                self._drone1_tsp_path = [tid for tid in old_path if tid not in detected_ids] if old_path else []
                
                if len(old_path) != len(self._drone1_tsp_path):
                    removed_ids = set(old_path) - set(self._drone1_tsp_path)
                    print(f"[FSM] 1号无人机从TSP路径中移除目标: {removed_ids}")
                    print(f"[FSM] 1号无人机TSP路径中的目标ID: {self._drone1_tsp_path}")
                
                # 若索引超界，后续逻辑会触发返航，无需在此调整索引
                
                # 路径为空或已完成，开始返航
                if self._drone1_tsp_index >= len(self._drone1_tsp_path):
                    print(f"[FSM] 1号无人机完成所有目标访问，开始返航")
                    self.drone1_returning = True
                    return self._drone_return_to_parent_action(drone, observation)
                
                target_id = self._drone1_tsp_path[self._drone1_tsp_index]
                target_data = self.detected_targets.get(str(target_id))
                if not target_data:
                    self._drone1_tsp_index += 1
                    print(f"[FSM] 1号无人机目标{target_id}丢失，跳到下一个")
                    return np.array([0.0, drone.heading, 1.0])
                
                pred_x, pred_y = target_data['predict_pos']
                if pred_x < self.blockade_line_x:
                    print(f"[FSM] 1号无人机目标 {target_id} 预测点在封锁线后，跳过此目标")
                    x, y = target_data['position']
                    print(f"[FSM] 目标当前位置: ({x:.0f}, {y:.0f}), 预测位置: ({pred_x:.0f}, {pred_y:.0f})")
                    self._drone1_tsp_index += 1
                    return np.array([0.0, drone.heading, 1.0])
                
                dx = pred_x - drone.x
                dy = pred_y - drone.y
                dist = math.hypot(dx, dy)
                if dist < 500:
                    # 到达后进行一次环扫（以最小转弯半径全速做切向飞行一步，近似完成一圈）
                    current_angle = math.atan2(drone.y - pred_y, drone.x - pred_x)
                    tangent_angle = current_angle + math.pi / 2.0
                    circle_speed = drone.max_speed * 0.8
                    self._drone1_tsp_index += 1
                    if self._drone1_tsp_index >= len(self._drone1_tsp_path):
                        print(f"[FSM] 1号无人机完成所有目标访问，开始返航")
                        self.drone1_returning = True
                        return self._drone_return_to_parent_action(drone, observation)
                    return np.array([circle_speed, tangent_angle, 1.0])
                nav_heading = math.atan2(dy, dx)
                nav_speed = drone.max_speed
                return np.array([nav_speed, nav_heading, 1.0])
            else:
                # 1号机任务已完成，保持降落
                return np.array([0.0, drone.heading, 0.1])
        
        # 3号机执行逻辑
        elif drone.id == 3:
            # 延迟出动：1号机起飞后6000秒
            if self.drone1_launch_time is not None and current_time - self.drone1_launch_time < 5000.0:
                return np.array([0.0, drone.heading, 0.1])
            if self.patrol_drone_sequence == "drone_3_active":
                # 确保3号机TSP路径（全目标）已生成
                if self._drone3_tsp_path is None:
                    print(f"[FSM] 3号无人机TSP路径（全目标）未生成，重新生成")
                    self._create_drone3_patrol_path_all_targets(observation)
                    self._drone3_tsp_path = self._drone3_tsp_path or []
                    self._drone3_tsp_index = 0
                
                # 动态移除已探测目标（统一为字符串ID以匹配TSP路径中的字符串ID）
                detected_ids = set(str(t.id) for t in observation.get_single_detected_targets(drone))
                old_path = self._drone3_tsp_path.copy() if self._drone3_tsp_path else []
                self._drone3_tsp_path = [tid for tid in old_path if tid not in detected_ids] if old_path else []
                
                if len(old_path) != len(self._drone3_tsp_path):
                    removed_ids = set(old_path) - set(self._drone3_tsp_path)
                    print(f"[FSM] 3号无人机从TSP路径中移除目标: {removed_ids}")
                    print(f"[FSM] 3号无人机TSP路径中的目标ID: {self._drone3_tsp_path}")
                
                # 若索引超界，后续逻辑会触发返航，无需在此调整索引
                
                # 路径为空或已完成
                if self._drone3_tsp_index >= len(self._drone3_tsp_path):
                    print(f"[FSM] 3号无人机完成所有目标访问，开始返航")
                    return self._drone_return_to_parent_action(drone, observation)
                
                target_id = self._drone3_tsp_path[self._drone3_tsp_index]
                target_data = self.detected_targets.get(str(target_id))
                if not target_data:
                    self._drone3_tsp_index += 1
                    print(f"[FSM] 3号无人机目标{target_id}丢失，跳到下一个")
                    return np.array([0.0, drone.heading, 1.0])
                
                pred_x, pred_y = target_data['predict_pos']
                # 对3号机：允许访问封锁线后的目标，不跳过
                
                dx = pred_x - drone.x
                dy = pred_y - drone.y
                dist = math.hypot(dx, dy)
                if dist < 500:
                    # 到达后：执行完整一圈盘旋，完成后再推进到下一个TSP目标
                    circle = self._patrol_drone3_circle
                    if not circle['active']:
                        circle['active'] = True
                        circle['center'] = (pred_x, pred_y)
                        circle['last_angle'] = None
                        circle['accum_angle'] = 0.0
                        circle['radius_m'] = 300.0
                    # 执行一帧盘旋
                    cx, cy = circle['center']
                    current_angle = math.atan2(drone.y - cy, drone.x - cx)
                    last_angle = circle['last_angle']
                    if last_angle is None:
                        circle['last_angle'] = current_angle
                        last_angle = current_angle
                    delta = current_angle - last_angle
                    if delta > math.pi:
                        delta -= 2 * math.pi
                    elif delta < -math.pi:
                        delta += 2 * math.pi
                    # 直接累计归一化后的角度差，避免大步长导致长期不累计
                    circle['accum_angle'] += abs(delta)
                    circle['last_angle'] = current_angle
                    # 完整一圈后推进索引
                    if circle['accum_angle'] >= 2 * math.pi * 0.8:
                        circle['active'] = False
                        self._drone3_tsp_index += 1
                        if self._drone3_tsp_index >= len(self._drone3_tsp_path):
                            print(f"[FSM] 3号无人机完成所有目标访问，开始返航")
                            return self._drone_return_to_parent_action(drone, observation)
                        # 完成一圈后先输出一个缓冲动作
                        return np.array([0.0, drone.heading, 1.0])
                    # 继续沿切向方向盘旋
                    tangent = current_angle + math.pi / 2.0
                    target_px = cx + circle['radius_m'] * math.cos(tangent)
                    target_py = cy + circle['radius_m'] * math.sin(tangent)
                    ddx = target_px - drone.x
                    ddy = target_py - drone.y
                    heading = math.atan2(ddy, ddx)
                    return np.array([drone.max_speed * 0.8, heading, 1.0])
                nav_heading = math.atan2(dy, dx)
                nav_speed = drone.max_speed
                return np.array([nav_speed, nav_heading, 1.0])
            else:
                # 3号机等待1号机完成任务，保持降落
                return np.array([0.0, drone.heading, 0.1])
        
        return np.array([0.0, 0.0, 1.0])

    # ==================== DEFENSIVE阶段函数 ====================
    
    def _execute_defensive_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行DEFENSIVE阶段的无人船行为 - 重构版本"""  
        # 清理无效目标
        self._cleanup_invalid_targets(boat, observation)
        # DEFENSIVE阶段滚动再分配（120s）
        self._maybe_defensive_reassign(observation)
        
        # 统一的目标选择和行为执行
        target_id, action_type = self._find_best_target(boat, observation)
        
        if target_id:
            return self._engage_target(boat, target_id, action_type, observation)
        else:
            return self._maintain_defensive_position(boat, observation)
    
    def _find_best_target(self, boat: WhiteBoat, observation: AgentObservation) -> Tuple[Optional[str], str]:
        """统一的目标选择逻辑，返回(target_id, action_type)"""
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 开始寻找新目标...")
        
        # 优先级1: 处理当前锁定目标
        if (boat.current_target is not None) and getattr(boat.current_target, 'active', False):
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级1: 继续锁定当前目标 {boat.current_target.id}")
            return str(boat.current_target.id), "current_lock"
        elif boat.current_target:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 当前目标 {boat.current_target.id} 已失效")
        
        # 优先级2: 处理无人机发现的近距离目标（新增）
        if boat.id in self.drone_close_target_alerts:
            alert = self.drone_close_target_alerts[boat.id]
            if not alert['handled']:
                target_id = alert['target_id']
                target = self._find_target_by_id(target_id, observation)
                if target and target.active:
                    # 验证目标确实在40km内
                    distance = boat.distance_to(target)
                    if distance <= 40000:
                        if GameConfig.debug_mode:
                            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 优先级2: 处理无人机发现的近距离目标 {target_id}")
                        # 将目标临时添加到船只的目标列表最前面（如果不在列表中）
                        boat_targets = self.boat_targets.get(boat.id, [])
                        tid = str(target_id)
                        if tid not in boat_targets:
                            boat_targets.insert(0, tid)
                            self.boat_targets[boat.id] = boat_targets
                            if GameConfig.debug_mode:
                                print(f"[FSM] DEFENSIVE: 将近距离目标 {target_id} 临时添加到船只 {boat.id} 目标列表")
                        return target_id, "drone_guided_close"
                
                # 目标无效，标记警报为已处理
                alert['handled'] = True
        
        # 优先级3: 处理45km探测范围内的威胁目标
        threats = self._check_black_boats_in_specific_white_45km_range(observation, boat)
        if threats:
            nearest_threat_id = self._find_nearest_threat(boat, [str(t) for t in threats])
            if nearest_threat_id:
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级3: 处理威胁目标 {nearest_threat_id}")
                return nearest_threat_id, "immediate_threat"
        
        # 优先级4: 处理分配的目标
        assigned_targets = self.boat_targets.get(boat.id, [])
        if assigned_targets:
            head = str(assigned_targets[0])
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级4: 处理分配的目标 {head}")
            return head, "assigned"
        else:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 没有分配的目标")
        
        # 优先级5: 处理可拦截的目标
        nearest_interceptable = self._find_nearest_interceptable_target(boat, observation)
        if nearest_interceptable:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级5: 处理可拦截目标 {nearest_interceptable}")
            return nearest_interceptable, "interceptable"
        
        # 优先级6: 处理探测到的目标（新增）- 在DEFENSIVE阶段直接处理探测到的目标
        if boat.detected_targets:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 探测到 {len(boat.detected_targets)} 个目标")
            # 选择最近的在锁定范围内的目标
            lockable_targets = []
            for target in boat.detected_targets:
                distance = boat.distance_to(target)
                if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
                    lockable_targets.append((target, distance))
            
            if lockable_targets:
                # 选择最近的目标
                nearest_target, _ = min(lockable_targets, key=lambda x: x[1])
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: 船只 {boat.id} 优先级6: 直接锁定探测到的目标 {nearest_target.id}")
                return str(nearest_target.id), "detected"
            else:
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: Boat {boat.id} 探测到的目标都不在锁定范围内")
        else:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 没有探测到任何目标")
        
        # 优先级7: 无条件尝试 - 选择未占用的最近目标，否则在所有预测目标中选择最近目标，尝试拦截点航行
        candidate_tid = self._find_unclaimed_or_nearest_target(boat, observation)
        if candidate_tid is not None:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 优先级7: 无条件尝试目标 {candidate_tid}")
            return str(candidate_tid), "attempt_unconditional"

        # 优先级8: 无目标，执行防御巡逻
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 所有优先级均未命中，无目标，返回防御巡逻")
        return None, "patrol"


    
    def _engage_target(self, boat: WhiteBoat, target_id: str, action_type: str, observation: AgentObservation) -> np.ndarray:
        """统一的目标交战逻辑"""
        # [清理] 当船只接到交战任务时，清除其可能存在的防御巡逻状态
        if boat.id in self.boat_patrol_circles:
            del self.boat_patrol_circles[boat.id]

        # 处理当前锁定目标的特殊逻辑
        if action_type == "current_lock" and boat.current_target:
            return self._handle_locking_target(boat, boat.current_target, observation)

        # 对于其他类型目标，先查找目标对象
        target = self._find_target_by_id(target_id, observation)
        if not target or not target.active:
            # 若不可见/无效，但在detected_targets中仍有预测信息，则继续按预测追击，避免原地不动
            predicted = self.detected_targets.get(str(target_id))
            if predicted is not None:
                # DEFENSIVE增强：若该目标为队首且在预测点未见，可触发一次性验证盘旋
                try:
                    head_id = None
                    bt_list = self.boat_targets.get(boat.id, [])
                    if isinstance(bt_list, list) and bt_list:
                        head_id = str(bt_list[0])
                    if self.global_state == GlobalState.DEFENSIVE and head_id == str(target_id):
                        self._maybe_launch_validation_circle(boat, str(target_id), observation)
                except Exception:
                    pass
                if action_type == "attempt_unconditional":
                    # 无条件尝试：优先用拦截点尝试
                    inter = self._calculate_intercept_point(
                        boat,
                        predicted.get('position', predicted.get('predict_pos', (0.0, 0.0))),
                        float(predicted.get('speed', 0.0)),
                        float(predicted.get('heading', 0.0))
                    )
                    if inter:
                        (ix, iy), _, _ = inter
                        dx = ix - boat.x
                        dy = iy - boat.y
                        heading = math.atan2(dy, dx)
                        if GameConfig.debug_mode:
                            print(f"[FSM] DEFENSIVE: Boat {boat.id} 尝试拦截点前往目标 {target_id}（预测）")
                        try:
                            lock_id = int(target_id)
                        except Exception:
                            lock_id = -1
                        return np.array([boat.max_speed, heading, lock_id])
                    # 否则退化为基于预测点直航尝试
                    return self._pursue_predicted_target(boat, str(target_id), observation)
                else:
                    if GameConfig.debug_mode:
                        print(f"[FSM] DEFENSIVE: 目标 {target_id} 当前不可见，使用预测信息继续追击")
                    return self._pursue_predicted_target(boat, str(target_id), observation)

            # 目标完全无效，清理并回到防御位置
            self._remove_invalid_target(boat, target_id)
            if action_type == "drone_guided_close" and boat.id in self.drone_close_target_alerts:
                self.drone_close_target_alerts[boat.id]['handled'] = True
            return self._maintain_defensive_position(boat, observation)
        
        # 处理无人机指引的近距离目标（特殊处理）
        if action_type == "drone_guided_close":
            return self._handle_drone_guided_close_target(boat, target, observation)

        # 无条件尝试：优先计算最短拦截点；若失败则直追当前位置
        if action_type == "attempt_unconditional":
            inter = self._calculate_intercept_point(
                boat,
                (target.x, target.y),
                float(target.speed),
                float(target.heading)
            )
            if inter:
                (ix, iy), _, _ = inter
                dx = ix - boat.x
                dy = iy - boat.y
                heading = math.atan2(dy, dx)
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: Boat {boat.id} 尝试拦截点前往目标 {target.id}")
                return np.array([boat.max_speed, heading, target.id])
            # 拦截点失败则直追
            dx = target.x - boat.x
            dy = target.y - boat.y
            heading = math.atan2(dy, dx)
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 拦截点失败，改为直追目标 {target.id}")
            return np.array([boat.max_speed, heading, target.id])
        
        # 根据距离决定行为策略
        distance = boat.distance_to(target)
        
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE :
            # 在锁定范围内，开始锁定
            return self._initiate_lock(boat, target, action_type, observation)
        else:
            # 超出锁定范围，计算追击
            return self._pursue_target_unified(boat, target, action_type, observation)

    def _pursue_predicted_target(self, boat: WhiteBoat, target_id: str, observation: AgentObservation) -> np.ndarray:
        """基于detected_targets的预测信息追击指定目标（用于DEFENSIVE下目标临时不可见时）"""
        data = self.detected_targets.get(str(target_id))
        if not data:
            return self._maintain_defensive_position(boat, observation)

        # 优先使用极限拦截点（入圈最早）
        intercept = self._calculate_intercept_point(
            boat,
            data.get('position', data.get('predict_pos', (0.0, 0.0))),
            float(data.get('speed', 0.0)),
            float(data.get('heading', 0.0))
        )
        if intercept:
            (ix, iy), _, _ = intercept
            dx = ix - boat.x
            dy = iy - boat.y
            heading = math.atan2(dy, dx)
            try:
                lock_id = int(target_id)
            except Exception:
                lock_id = -1
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 追击预测目标 {target_id}（拦截点）")
            return np.array([boat.max_speed, heading, lock_id])

        # 回退：直航到预测位置
        px, py = data.get('predict_pos', data.get('position', (boat.x, boat.y)))
        dx = px - boat.x
        dy = py - boat.y
        heading = math.atan2(dy, dx)
        try:
            lock_id = int(target_id)
        except Exception:
            lock_id = -1
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 追击预测目标 {target_id}（直航预测点）")
        return np.array([boat.max_speed, heading, lock_id])
    
    def _cleanup_invalid_targets(self, boat: WhiteBoat, observation: AgentObservation):
        """清理无效目标：仅当目标既不可见又不在预测列表中才移除"""
        assigned_targets = self.boat_targets.get(boat.id, [])
        visible_ids = set(str(t.id) for t in observation.black_boats if t.active)
        active_targets: List[str] = []
        for target_id in assigned_targets:
            tid_str = str(target_id)
            if tid_str in visible_ids:
                active_targets.append(target_id)
                continue
            if tid_str in self.detected_targets:  # 保留仍有预测信息的目标（字符串键）
                active_targets.append(target_id)
                continue
            # 两者都没有，才认为无效
        removed_targets = set(assigned_targets) - set(active_targets)
        for target_id in removed_targets:
            print(f"[FSM] DEFENSIVE: 清理船只 {boat.id} 的无效目标 {target_id}")
        self.boat_targets[boat.id] = active_targets
    
    def _remove_invalid_target(self, boat: WhiteBoat, target_id: str):
        """移除单个无效目标"""
        assigned_targets = self.boat_targets.get(boat.id, [])
        if target_id in assigned_targets:
            assigned_targets.remove(target_id)
            self.boat_targets[boat.id] = assigned_targets
            print(f"[FSM] DEFENSIVE: 移除船只 {boat.id} 的无效目标 {target_id}")
    
    def _initiate_lock(self, boat: WhiteBoat, target: BlackBoat, action_type: str, observation: AgentObservation) -> np.ndarray:
        """开始锁定目标"""
        boat.current_target = target
        
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 开始锁定目标 {target.id}，此时距离为{boat.distance_to(target):.0f}m")
        
        # 检查目标状态并执行相应动作
        if target.is_frozen(observation.current_time):
            return np.array([0.0, target.heading, target.id])
        else:
            return np.array([target.speed, target.heading, target.id])
    
    def _pursue_target_unified(self, boat: WhiteBoat, target: BlackBoat, action_type: str, observation: AgentObservation) -> np.ndarray:
        """统一的目标追击逻辑"""
        drone_action = 1.0 if action_type == "immediate_threat" else 0.0
        
        # 计算会合/入圈点
        rendezvous = self._compute_rendezvous(boat, target)
        if rendezvous is not None:
            rx, ry = rendezvous
            dx = rx - boat.x
            dy = ry - boat.y
            heading = math.atan2(dy, dx)
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 前往会合点切入锁定环 目标 {target.id}")
            return np.array([boat.max_speed, heading, target.id])

        # 回退：使用极限拦截点
        intercept_result = self._calculate_intercept_point(
            boat, (target.x, target.y), target.speed, target.heading
        )
        if intercept_result:
            intercept_point, _, _ = intercept_result
            dx = intercept_point[0] - boat.x
            dy = intercept_point[1] - boat.y
            heading = math.atan2(dy, dx)
            return np.array([boat.max_speed, heading, target.id])
        
        # 兜底：直接朝目标当前位置追击，避免原地不动
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 无法计算会合/拦截点，改为直接追击目标 {target.id}")
        dx = target.x - boat.x
        dy = target.y - boat.y
        heading = math.atan2(dy, dx)
        return np.array([boat.max_speed, heading, target.id])
    
    def _find_nearest_threat(self, boat: WhiteBoat, threats: List[str]) -> Optional[str]:
        """从威胁列表中找到最近的一个"""
        min_dist = float('inf')
        nearest_threat_id = None
        for threat_id in threats:
            target_data = self.detected_targets.get(threat_id)
            if target_data:
                # 使用预测位置计算距离
                dist = math.hypot(
                    boat.x - target_data.get('predict_pos', target_data['position'])[0],
                    boat.y - target_data.get('predict_pos', target_data['position'])[1]
                )
                if dist < min_dist:
                    min_dist = dist
                    nearest_threat_id = threat_id
        return nearest_threat_id

    def _find_unclaimed_or_nearest_target(self, boat: WhiteBoat, observation: AgentObservation) -> Optional[str]:
        """在detected_targets中挑选：优先未被任何白艇占用的最近目标；若无，则在全部目标中选择最近目标。
        占用定义：在任一船的目标列表中或被任一船当前锁定。
        返回字符串unit_id。"""
        if not self.detected_targets:
            return None

        # 构造占用集（字符串id）
        occupied: Set[str] = set()
        for bid, ids in self.boat_targets.items():
            if isinstance(ids, list):
                occupied.update(str(tid) for tid in ids)
        for wb in observation.white_boats:
            if wb.current_target is not None:
                occupied.add(str(wb.current_target.id))

        # 采样数据：预测点及到当前船距离
        candidates = []  # (tid_str, px, py, dist)
        now = observation.current_time
        for tid, data in self.detected_targets.items():
            # 预测点优先，没有则用position
            px, py = data.get('predict_pos', data.get('position', (0.0, 0.0)))
            dist = math.hypot(px - boat.x, py - boat.y)
            candidates.append((str(tid), float(px), float(py), float(dist)))

        if not candidates:
            return None

        # 未占用子集
        unclaimed = [c for c in candidates if c[0] not in occupied]

        pick_from = unclaimed if unclaimed else candidates
        if not pick_from:
            return None

        # 选最近
        best = min(pick_from, key=lambda x: x[3])
        return best[0]
    
    def _handle_drone_guided_close_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理无人机指引的近距离目标"""
        distance = boat.distance_to(target)
        
        # 标记警报为已处理
        if boat.id in self.drone_close_target_alerts:
            self.drone_close_target_alerts[boat.id]['handled'] = True
        
        print(f"[FSM] DEFENSIVE: 船只 {boat.id} 处理无人机指引的近距离目标 {target.id} (距离: {distance:.0f}m)")
        
        # 保存原定目标信息（如果存在）
        original_target_id = None
        boat_target_list = self.boat_targets.get(boat.id, [])
        if isinstance(boat_target_list, list) and boat_target_list:
            original_target_id = str(boat_target_list[0])  # 统一为字符串ID
        
        # 立即尝试锁定近距离目标，暂停原定任务
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE - 100:
            # 在锁定范围内，开始锁定
            result = self._initiate_lock(boat, target, "drone_guided_close", observation)
            
            # 检查目标是否被击沉
            if not target.active:
                # 目标被击沉，恢复原定目标并重新计算拦截路线
                self._restore_original_target_after_close_engagement(boat, original_target_id, observation)
            
            return result
        else:
            # 超出锁定范围，追击
            return self._pursue_target_unified(boat, target, "drone_guided_close", observation)

    def _restore_original_target_after_close_engagement(self, boat: WhiteBoat, original_target_id: Optional[str], observation: AgentObservation):
        """在处理完近距离目标后恢复原定目标并重新计算拦截路线"""
        if original_target_id:
            # 检查原定目标是否仍然存在
            original_target = self._find_target_by_id(original_target_id, observation)
            if original_target and original_target.active:
                print(f"[FSM] DEFENSIVE: 船只 {boat.id} 处理完近距离目标，恢复原定目标 {original_target_id}")
                
                # 重新计算拦截路线
                target_data = self.detected_targets.get(str(original_target_id))
                if target_data:
                    intercept_result = self._calculate_intercept_point(
                        boat, target_data.get('predict_pos', target_data['position']), 
                        target_data['speed'], 
                        target_data['heading']
                    )
                    if intercept_result:
                        intercept_point, intercept_time, _ = intercept_result
                        print(f"[FSM] DEFENSIVE: 船只 {boat.id} 重新计算目标 {original_target_id} 的拦截路线")
                        print(f"                新拦截点: ({intercept_point[0]:.0f}, {intercept_point[1]:.0f})")
            else:
                # 原定目标已不存在，从目标列表中移除
                boat_target_list = self.boat_targets.get(boat.id, [])
                if isinstance(boat_target_list, list) and str(original_target_id) in boat_target_list:
                    boat_target_list.remove(str(original_target_id))
                    print(f"[FSM] DEFENSIVE: 船只 {boat.id} 原定目标 {original_target_id} 已不存在，从列表中移除")
                
                # 转向下一个目标
                next_target_id = self._get_next_target_for_boat(boat)
                if next_target_id:
                    print(f"[FSM] DEFENSIVE: 船只 {boat.id} 转向下一个目标 {next_target_id}")
        else:
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 无原定目标，保持警戒位置")
    
    def _handle_locking_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理正在锁定的目标"""
        
        
        # 检查目标状态
        target_status = self._check_target_status(target, observation, boat)
        
        if target_status == "destroyed":
            return self._handle_target_destroyed(boat, target, observation)
        elif target_status == "frozen":
            return self._handle_target_frozen(target)
        elif target_status == "in_range":
            return self._handle_target_in_range(target)
        else:  # out_of_range
            return self._handle_target_out_of_range(boat, target, observation,)
    
    def _check_target_status(self, target: BlackBoat, observation: AgentObservation, boat: WhiteBoat) -> str:
        """检查目标状态"""
        if not target.active:
            return "destroyed"
        if target.is_frozen(observation.current_time):
            return "frozen"
        
        distance = boat.distance_to(target)
        if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
            return "in_range"
        else:
            return "out_of_range"
    
    def _handle_target_destroyed(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理目标被击沉的情况 - 更新后的版本"""
        print(f"[FSM] DEFENSIVE: Boat {boat.id} 成功击沉目标 {target.id}")
        
        # 清理无人艇的current_target，确保它能转向新目标
        if boat.current_target and boat.current_target.id == target.id:
            boat.current_target = None
            boat.stop_locking()
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 清理已击沉目标的锁定状态")
        
        target_id = str(target.id)
        
        # 从任务分配中移除
        self._remove_target_from_assignments(boat, target_id)
        
        # 记录击沉事件
        self._handle_target_destroyed_notification(target_id, boat.id, observation.current_time)
        
        # 寻找新目标或保持警戒
        return self._find_new_target_or_patrol(boat, observation)
    
    def _handle_target_frozen(self, target: BlackBoat) -> np.ndarray:
        """处理目标被冻结的情况"""
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: 目标 {target.id} 已冻结，继续锁定击沉")
        return np.array([0.0, target.heading, target.id])
    
    def _handle_target_in_range(self, target: BlackBoat) -> np.ndarray:
        """处理目标在锁定范围内的情况"""
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: 锁定目标 {target.id}，保持相同航向速度")
        return np.array([target.speed, target.heading, target.id])
    
    def _handle_target_out_of_range(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """处理目标超出锁定范围的情况"""
        print(f"[FSM] DEFENSIVE: 目标 {target.id} 超出锁定范围，追击")
        return self._pursue_target(boat, target, observation)
    
    def _remove_target_from_assignments(self, boat: WhiteBoat, target_id: str):
        """从分配列表中移除目标"""
        boat_target_list = self.boat_targets.get(boat.id, [])
        if isinstance(boat_target_list, list) and str(target_id) in boat_target_list:
            boat_target_list.remove(str(target_id))
            self.boat_targets[boat.id] = boat_target_list
            print(f"[FSM] DEFENSIVE: 目标 {target_id} 已从船只 {boat.id} 的任务列表中移除")
        
        # 从探测目标中删除
        if str(target_id) in self.detected_targets:
            del self.detected_targets[str(target_id)]
    
    def _find_new_target_or_patrol(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """寻找新目标或保持巡逻"""
        # 使用统一的目标选择逻辑
        target_id, action_type = self._find_best_target(boat, observation)
        if target_id:
            return self._engage_target(boat, target_id, action_type, observation)
        else:
            return self._maintain_defensive_position(boat, observation)
    
    def _pursue_target(self, boat: WhiteBoat, target: BlackBoat, observation: AgentObservation) -> np.ndarray:
        """追击目标"""
        # 计算追击航向
        intercept_result = self._calculate_intercept_point(boat, (target.x, target.y), target.speed, target.heading)
        if intercept_result:
            intercept_point, _, _ = intercept_result
            dx = intercept_point[0] - boat.x
            dy = intercept_point[1] - boat.y
            heading = math.atan2(dy, dx)
            speed = boat.max_speed
            return np.array([speed, heading, target.id])
        else:
            # 无法计算拦截点，保持防御位置
            print(f"[FSM] DEFENSIVE: Boat {boat.id} 无法计算目标 {target.id} 的拦截点")
            return self._maintain_defensive_position(boat, observation)

    def _find_target_by_id(self, target_id: str, observation: AgentObservation) -> Optional[BlackBoat]:
        """根据ID查找目标"""
        try:
            # 尝试将target_id转换为整数进行匹配
            target_id_int = int(target_id)
            return next((t for t in observation.black_boats if t.id == target_id_int), None)
        except ValueError:
            # 如果转换失败，尝试字符串匹配
            return next((t for t in observation.black_boats if str(t.id) == target_id), None)
    
    def _find_nearest_interceptable_target(self, boat: WhiteBoat, observation: AgentObservation) -> Optional[str]:
        """寻找最近的可拦截目标"""
        nearest_target_id = None
        min_intercept_time = float('inf')
        
        # 检查当前检测到的目标
        for target_id, target_data in self.detected_targets.items():
            if not target_data.get('active', True):
                continue
                
            # 使用预测位置计算拦截时间
            intercept_result = self._calculate_intercept_point(
                boat, target_data.get('predict_pos', target_data['position']), 
                target_data['speed'], 
                target_data['heading']
            )
            if intercept_result:
                _, intercept_time, _ = intercept_result
                if intercept_time < min_intercept_time:
                    min_intercept_time = intercept_time
                    nearest_target_id = target_id
        
        # 如果detected_targets中没有找到目标，检查observation.black_boats中的活跃目标
        if nearest_target_id is None:
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 在detected_targets中没有找到可拦截目标，检查observation.black_boats")
            for target in observation.black_boats:
                if target.active:
                    # 计算拦截时间
                    intercept_result = self._calculate_intercept_point(
                        boat, (target.x, target.y), 
                        target.speed, 
                        target.heading
                    )
                    if intercept_result:
                        _, intercept_time, _ = intercept_result
                        if intercept_time < min_intercept_time:
                            min_intercept_time = intercept_time
                            nearest_target_id = str(target.id)
                            print(f"[FSM] DEFENSIVE: Boat {boat.id} 在observation.black_boats中找到可拦截目标 {target.id}")
        
        return nearest_target_id
    
    def _maintain_defensive_position(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """保持防御位置（无目标时）：在部署线最近巡逻中心小半径环巡，避免静止聚集"""
        # 仅在DEFENSIVE阶段启用巡逻逻辑
        if self.global_state != GlobalState.DEFENSIVE:
            return np.array([0.0, boat.heading, -1])

        # 若该船仍然有目标，兜底保持原地警戒（正常流程中不会走到此分支）
        if self._has_target(boat):
            return np.array([0.0, boat.heading, -1])

        # 确保巡逻中心分配已建立（先预留有目标船，再为无目标船分配）
        self._rebuild_defensive_patrol_assignments(observation)

        # 取得该船巡逻中心，如缺失则回退选择最近中心
        center = self._defensive_patrol_assignments.get(boat.id)
        if center is None:
            centers = self._compute_deployment_centers(5)
            if not centers:
                return np.array([0.0, boat.heading, -1])
            # 选择最近中心
            cx, cy = min(centers, key=lambda c: math.hypot(c[0] - boat.x, c[1] - boat.y))
            center = (cx, cy)
            self._defensive_patrol_assignments[boat.id] = center

        cx, cy = center

        # 在DEFENSIVE阶段主动发射无人机增强探测（保持原行为）
        _ = self._should_launch_drone_for_defense(boat, observation)

        # 构建或更新该船的环巡状态
        state = self.boat_patrol_circles.get(boat.id, {
            'center': (cx, cy),
            'radius_m': float(self.idle_patrol_radius_m),
            'last_angle': None,
        })
        # 若中心变化则重置轨道角度
        if state.get('center') != (cx, cy):
            state['center'] = (cx, cy)
            state['last_angle'] = None

        radius_m: float = float(state.get('radius_m', self.idle_patrol_radius_m))

        # 计算与中心的几何量
        dx_c = boat.x - cx
        dy_c = boat.y - cy
        dist_c = math.hypot(dx_c, dy_c)

        # 若与中心距离偏差较大，先调整到轨道半径上
        desired_r = radius_m
        if not math.isfinite(dist_c) or desired_r <= 0:
            return np.array([0.0, boat.heading, -1])

        # 距离公差：20%半径
        tol = max(0.2 * desired_r, 100.0)
        if abs(dist_c - desired_r) > tol:
            # 径向移动至目标半径位置
            if dist_c < 1e-3:
                # 正在中心点，先向上方平移至轨道上
                target_x = cx + desired_r
                target_y = cy
            else:
                scale = desired_r / dist_c
                target_x = cx + dx_c * scale
                target_y = cy + dy_c * scale
            hd = math.atan2(target_y - boat.y, target_x - boat.x)
            spd = boat.max_speed * self.idle_patrol_speed_ratio
            return np.array([spd, hd, -1])

        # 已在轨道附近：沿切向飞行
        current_angle = math.atan2(boat.y - cy, boat.x - cx)
        last_angle = state.get('last_angle')
        if last_angle is None:
            state['last_angle'] = current_angle
            last_angle = current_angle

        tangent_angle = current_angle + math.pi / 2.0
        self.boat_patrol_circles[boat.id] = state

        speed = float(boat.max_speed * self.idle_patrol_speed_ratio)
        return np.array([speed, tangent_angle, -1])

    def _maybe_defensive_reassign(self, observation: AgentObservation):
        """DEFENSIVE阶段滚动再分配"""
        now = observation.current_time
        if now - self._defense_last_reassign_time >= self.defense_reassign_interval:
            print(f"[FSM] DEFENSIVE: 触发滚动再分配，时间: {now}")
            try:
                self._defensive_build_assignment(observation)
            finally:
                self._defense_last_reassign_time = now

    def _defensive_build_assignment(self, observation: AgentObservation):
        """基于ETA/ΔY/威胁评分的防御再分配，更新 boat_targets"""
        white_boats = [b for b in observation.white_boats if b.active]
        if not white_boats:
            return
        # 构造目标集合
        targets: List[Dict] = []
        for tid, data in self.detected_targets.items():
            targets.append({
                'id': tid,
                'position': data.get('predict_pos', data['position']),
                'predict_pos': data.get('predict_pos', data['position']),
                'speed': data['speed'],
                'heading': data['heading']
            })
        if not targets:
            # 如果没有目标，也应该清空现有任务，让所有船只进入警戒状态
            self.boat_targets.clear()
            print("[FSM] DEFENSIVE: 无可分配目标，已清空所有任务，所有船只将进入警戒状态")
            return
        
        # 在重新计算前，清空所有船只的旧任务分配
        self.boat_targets.clear()
        print("[FSM] DEFENSIVE: 防御阶段重分配，已清空所有旧任务")

        # 简化：复用ETA分配器（α·ETA + β·|ΔY|），再以威胁评分做次序微调
        new_assignment = self._assign_tasks_with_eta(targets, white_boats)
        # 抖动抑制：若切换队首带来的ETA改善<阈值，则保留原队首
        for boat in white_boats:
            bid = boat.id
            new_list = new_assignment.get(bid, [])
            if not new_list:
                continue
            # 新/旧队首对比
            new_head = new_list[0]
            old_ids = self.boat_targets.get(bid, [])
            if old_ids:
                old_head_id = old_ids[0]
                # 估算旧队首ETA
                # 在targets中找到旧队首的条目
                old_info = next((t for t in targets if str(t['id']) == str(old_head_id)), None)
                if old_info is not None:
                    old_eta = self._estimate_eta(boat, old_info)
                    new_eta = float(new_head.get('eta', float('inf')))
                    if old_eta != float('inf') and new_eta != float('inf'):
                        improvement = (old_eta - new_eta) / max(old_eta, 1e-6)
                        if improvement < self.defense_eta_switch_threshold:
                            # 保留旧队首，将新队首插入第二位，减少抖动
                            new_ids = [str(old_head_id)] + [str(t['id']) for t in new_list if str(t['id']) != str(old_head_id)]
                            self.boat_targets[bid] = new_ids
                            continue
            # 默认采用新分配顺序
            self.boat_targets[bid] = [str(t['id']) for t in new_list]
        # 更新时间戳
        if self.task_manager.last_assignment:
            self.task_manager.last_assignment['timestamp'] = observation.current_time

    def _compute_rendezvous(self, boat: WhiteBoat, target: BlackBoat) -> Optional[Tuple[float, float]]:
        """选取会合点：优先在锁定环半径内的目标轨迹上点，保证切入稳定"""
        # 预测目标未来若干时间的轨迹，寻找距船只距离落在锁定环半径区间的点
        ring_min, ring_max = self.lock_ring_radius
        # 以简单步进预测（步长30s、总时长1200s，提高命中率）
        step = 30.0
        horizon = 1200.0
        t = 0.0
        while t <= horizon:
            px = target.x + target.speed * math.cos(target.heading) * t
            py = target.y + target.speed * math.sin(target.heading) * t
            d = math.hypot(px - boat.x, py - boat.y)
            if ring_min <= d <= ring_max:
                return (px, py)
            t += step
        return None
    
    def _should_launch_drone_for_defense(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """DEFENSIVE阶段是否应该发射无人机增强探测"""
        # 在DEFENSIVE阶段，主动发射无人机增强探测能力
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging" ):
                print(f"[FSM] DEFENSIVE: Boat {boat.id} 主动发射无人机 {drone.id} 增强探测")
                return 1.0  # 发射无人机
        
        return 0.0  # 没有可用无人机
   
    # ==================== 无人机相关函数 ====================
    
    def _generate_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """根据全局状态生成无人机动作"""
        # DEFENSIVE阶段：所有无人机可以起飞，跟踪或搜索
        if self.global_state == GlobalState.DEFENSIVE:
            return self._execute_defensive_drone_action(drone, observation)
        
        # 2号无人机执行任务，其余无人机PATROL阶段全部返航
        if self.global_state == GlobalState.PATROL:
            # 若该无人机被安排了换位代飞任务，则优先执行
            swap_task = self.drone_swap_tasks.get(drone.id)
            if swap_task and swap_task.get('type') == 'fly_to_boat':
                target_boat_id = swap_task.get('target_boat_id')
                # 引导该无人机飞向目标母舰位置
                target_boat = next((b for b in observation.white_boats if b.id == target_boat_id and b.active), None)
                if target_boat is not None:
                    dx = target_boat.x - drone.x
                    dy = target_boat.y - drone.y
                    heading = math.atan2(dy, dx)
                    # 当接近目标母舰则清除任务并降落
                    if math.hypot(dx, dy) < 5000:
                        # 清理任务，转为常规返航到该母舰
                        self.drone_swap_tasks.pop(drone.id, None)
                        return np.array([drone.max_speed, heading, 0.3])
                    return np.array([drone.max_speed, heading, 0.8])
                else:
                    # 目标船无效，清理任务
                    self.drone_swap_tasks.pop(drone.id, None)

            if drone.id == 1 or drone.id == 3:
                return self._execute_patrol_drone_action(drone, observation)
            else:
                return self._drone_return_to_parent_action(drone, observation)
        
        # 其余阶段原有逻辑
        if self.global_state == GlobalState.DEPLOYMENT:
            return self._execute_deployment_drone_mission(drone, observation)
        else:
            return np.array([drone.min_speed, 1.57, 0.6])  # 保持飞行 (action >= 0.5)
    
    def _execute_defensive_drone_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        执行DEFENSIVE阶段的无人机行为 - 完整的警戒扫视策略
        
        策略说明：
        - 无人机只有探测能力，无锁定和攻击能力
        
        策略流程：
        1. 进入DEFENSIVE阶段后，无人机先进行一次警戒扫视（以最小转弯半径飞一圈）
        2. 发现目标后根据距离和分配情况决定行为：
           ①目标在距离本无人船40km外，且在本舰本来的目标列表中：
              - 若无人机能量≥25% 且 目标距母舰≤200km：持续跟踪监视目标直到击沉，然后回母舰充电
              - 若无人机能量<25% 或 目标距母舰>200km：不进入持续监视，改为继续执行警戒扫视（完成该圈后按原逻辑返航/等待周期扫视）
           ②目标在距离本无人船40km外，且不在本舰本来的目标列表中：无人机回母舰充电，无人船继续向着既定目标前进
           ③目标在距离本无人船40km内：无人机指引无人船发现目标（无人船对目标进行锁定），然后无人机回母舰充电
        3. 无人机在充电时要每隔300s执行一次警戒扫视
        4. 无人船完成对目标列表中目标的击沉后，把目标从自己的目标中删去，转向列表中的下一个目标
        5. 列表清空后，每隔300s执行一次警戒扫视
        """
        current_time = observation.current_time
        drone_id = drone.id
        
        # 初始化无人机警戒状态
        if drone_id not in self.drone_alert_state:
            self.drone_alert_state[drone_id] = {
                'state': 'charging',
                'last_alert_time': 0,
                'target_id': None,
                'scan_center': None,
                'scan_angle': 0,
                'initial_scan_done': False  # 是否完成了进入DEFENSIVE阶段后的初始扫视
            }
        
        alert_state = self.drone_alert_state[drone_id]
        parent_boat = self._find_parent_boat(drone, observation)
        
        # 如果找不到母舰，保持当前状态
        if not parent_boat:
            return np.array([0.0, drone.heading, 1.0])
        
        # 能量检查 - 低于15%时强制返航
        if drone.energy < 10.0:
            alert_state['state'] = 'charging'
            print(f"[FSM] DEFENSIVE: 无人机 {drone_id} 能量不足({drone.energy:.1f}%)，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 如果无人机停靠在母舰上，优先处理验证盘旋请求
        if drone.docked_on_boat_id != -1:
            if alert_state.get('state') == 'one_off_circle':
                # 起飞进入一次性盘旋（含验证盘旋）
                alert_state['last_alert_time'] = current_time
                return np.array([20.0, drone.heading, 0.6])
            # 检查是否需要进行警戒扫视
            time_since_last_alert = current_time - alert_state['last_alert_time']
            
            # 需要警戒扫视的条件：
            # 1. 初始扫视未完成，或
            # 2. 距离上次警戒扫视已过300秒
            need_alert_scan = (not alert_state['initial_scan_done'] or time_since_last_alert >= 300)
            
            if need_alert_scan:
                # 起飞进行警戒扫视
                alert_state['state'] = 'alert_scanning'
                alert_state['scan_center'] = (parent_boat.x, parent_boat.y)  # 以母舰位置为中心
                alert_state['scan_angle'] = 0
                alert_state['last_alert_time'] = current_time
                print(f"[FSM] DEFENSIVE: 无人机 {drone_id} 开始警戒扫视 (初始:{not alert_state['initial_scan_done']})")
                return np.array([20.0, drone.heading, 0.6])  # 起飞 (action >= 0.5)
            else:
                # 在充电，等待下次扫视时间
                return np.zeros(3)
        
        # 警戒扫视状态
        if alert_state['state'] == 'alert_scanning':
            return self._execute_alert_scanning(drone, alert_state, parent_boat, observation)
        
        # 监视目标状态 - 持续监视分配给母舰的远距离目标
        elif alert_state['state'] == 'monitoring':
            return self._execute_target_monitoring(drone, alert_state, parent_boat, observation)
        
        # 指引母舰锁定状态 - 指引母舰锁定近距离目标
        elif alert_state['state'] == 'guiding_lock':
            return self._execute_guiding_lock(drone, alert_state, parent_boat, observation)
        
        # 一次性盘旋状态 - 对非本舰目标在预测点单圈盘旋后返航
        elif alert_state.get('state') == 'one_off_circle':
            return self._execute_one_off_circle(drone, alert_state, observation)
        
        # 默认返回充电状态
        return self._drone_return_to_parent_action(drone, observation)

    def _assist_locking_operation(self, drone: WhiteDrone, target: BlackBoat, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """辅助母舰的锁定操作"""
        # 检查目标是否已被冻结
        if target.is_frozen(observation.current_time):
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 辅助锁定，目标 {target.id} 已冻结")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 目标未冻结，继续辅助跟踪
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 辅助母舰锁定目标 {target.id}")
        return self._track_target_for_detection(drone, target)
    
    def _track_target_for_detection(self, drone: WhiteDrone, target: BlackBoat) -> np.ndarray:
        """跟踪目标以保持探测（无人机只负责探测，不攻击）"""
        dx = target.x - drone.x
        dy = target.y - drone.y
        distance = math.hypot(dx, dy)
        
        # 保持在合适的探测距离（不要太近也不要太远）
        ideal_distance = 3000  # 3km理想探测距离
        
        if distance < ideal_distance * 0.5:
            # 太近了，拉开距离以保持安全
            speed = drone.max_speed
            # 向目标相反方向移动
            heading = math.atan2(-dy, -dx)
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 距离目标过近，拉开距离")
        elif distance > ideal_distance * 2:
            # 太远了，接近目标以保持探测
            speed = drone.max_speed
            heading = math.atan2(dy, dx)
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 接近目标以保持探测")
        else:
            # 距离合适，保持跟随
            # 预测目标移动并跟随
            pred_x = target.x + target.speed * math.cos(target.heading) * 2  # 预测2秒后位置
            pred_y = target.y + target.speed * math.sin(target.heading) * 2
            dx_pred = pred_x - drone.x
            dy_pred = pred_y - drone.y
            heading = math.atan2(dy_pred, dx_pred)
            speed = min(drone.max_speed, target.speed * 1.2)  # 略快于目标
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 跟踪目标 {target.id} (距离: {distance:.0f}m)")
        
        return np.array([speed, heading, 1.0])  # 保持探测模式

    
    def _maintain_close_surveillance(self, drone: WhiteDrone, target: BlackBoat) -> np.ndarray:
        """对目标保持近距离监视（无人机只负责探测跟踪）"""
        dx = target.x - drone.x
        dy = target.y - drone.y
        distance = math.hypot(dx, dy)
        
        # 保持在合适距离监视目标
        ideal_distance = 30000  # 1km理想监视距离
        
        if distance > ideal_distance * 1.5:
            # 距离太远，接近目标
            heading = math.atan2(dy, dx)
            speed = min(drone.max_speed, distance / 5.0)
        elif distance < ideal_distance * 0.5:
            # 距离太近，拉开距离
            heading = math.atan2(-dy, -dx)
            speed = drone.max_speed * 0.5
        else:
            # 在合适距离，跟随目标移动
            # 预测目标位置并跟随
            pred_x = target.x + target.speed * math.cos(target.heading)
            pred_y = target.y + target.speed * math.sin(target.heading)
            dx_pred = pred_x - drone.x
            dy_pred = pred_y - drone.y
            heading = math.atan2(dy_pred, dx_pred)
            speed = target.speed
        
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 监视目标 {target.id} (距离: {distance:.0f}m)")
        return np.array([speed, heading, 1.0])
    

    
    def _execute_deployment_drone_mission(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        DEPLOYMENT阶段无人机任务：航点侦察
        
        任务流程：
        1. 前往预定义的航点
        2. 在航点进行圆形扫描侦察
        3. 处理探测到的目标信息
        4. 前往下一个航点
        5. 完成所有航点后返回母舰
        
        状态管理：
        - going_to_first_target: 前往第一个航点
        - circle_scanning: 在航点进行圆形扫描
        - going_to_next_target: 前往下一个航点
        - returning: 返回母舰
        - mission_completed: 任务完成
        """
        # 初始化任务状态
        mission_state = self.drone_mission_state.setdefault(drone.id, {
            'phase': 'going_to_first_target',      # 当前阶段
            'current_target': 0,                   # 当前航点索引
            'circle_progress': 0.0,                # 圆形扫描进度
            'circle_center': None,                 # 扫描中心点
            'circle_radius': 100,                  # 扫描半径（米）
            'accumulated_angle': 0.0,              # 累计扫描角度
            'last_angle': None,                    # 上次扫描角度
            'detected_at_waypoint': []             # 在航点探测到的目标列表
        })
        
        # 能量检查：低于20%时立即返回
        if drone.energy < 20.0:
            mission_state['phase'] = 'returning'
        

        if mission_state['phase'] == 'returning':
            # 任务完成，标记状态
            mission_state['phase'] = 'mission_completed'
        # 如果停靠在母舰上，检查任务状态
        if drone.docked_on_boat_id != -1:
            if mission_state['phase'] == 'mission_completed':
                return np.zeros(3)
        
        # 获取无人机的航点列表
        waypoints = self.drone_waypoints.get(drone.id, [])
        if drone.id == 1 or drone.id == 3:
            mission_state['phase'] = 'mission_completed'
            return np.array([0.0, 0.0, 0.0])
        # 执行航点任务
        return self._execute_waypoint_mission(drone, mission_state, waypoints, observation)
    
    def _execute_waypoint_mission(self, drone: WhiteDrone, mission_state: Dict, 
                                 waypoints: List[Tuple[float, float]], observation: AgentObservation) -> np.ndarray:
        """
        执行航点任务的具体逻辑
        
        阶段说明：
        1. going_to_first_target: 前往第一个航点
        2. circle_scanning: 在航点进行圆形扫描
        3. going_to_next_target: 前往下一个航点
        4. returning: 返回母舰
        
        Args:
            drone: 无人机对象
            mission_state: 任务状态字典
            waypoints: 航点列表
            observation: 环境观测
            
        Returns:
            np.ndarray: [速度, 航向, 无人机动作]
        """
        current_target = mission_state['current_target']
        phase = mission_state['phase']
        
        # 阶段1: 前往第一个航点
        if phase == 'going_to_first_target':
            if current_target < len(waypoints):
                target_x, target_y = waypoints[current_target]
                dx = target_x - drone.x
                dy = target_y - drone.y
                distance = math.sqrt(dx**2 + dy**2)
                # print(distance)
                
                if distance < 100:  # 到达航点（100米内）
                    # 切换到圆形扫描阶段
                    mission_state['phase'] = 'circle_scanning'
                    mission_state['circle_center'] = (target_x, target_y)
                    mission_state['circle_progress'] = 0.0
                    mission_state['accumulated_angle'] = 0.0
                    mission_state['last_angle'] = None
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
                else:
                    # 继续向航点移动
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
            else:
                # 没有更多航点，返回母舰
                mission_state['phase'] = 'returning'
                return self._drone_return_to_parent_action(drone, observation)
        
        # 阶段2: 圆形扫描
        elif phase == 'circle_scanning':
            scan_result = self._perform_circle_scan_fsm(drone, mission_state, observation)
            
            # 检查是否完成扫描
            if mission_state.get('scan_completed', False):
                # 完成当前航点扫描，处理探测到的目标
                self._process_detected_targets_at_waypoint(drone, mission_state, observation)
                
                # 前往下一个航点
                mission_state['current_target'] += 1
                mission_state['phase'] = 'going_to_next_target'
                mission_state['scan_completed'] = False
                
            return scan_result
        
        # 阶段3: 前往下一个航点
        elif phase == 'going_to_next_target':
            current_target = mission_state['current_target']
            if current_target < len(waypoints):
                target_x, target_y = waypoints[current_target]
                dx = target_x - drone.x
                dy = target_y - drone.y
                distance = math.sqrt(dx**2 + dy**2)

                if distance < 100:  # 到达下一个航点
                    # 切换到圆形扫描阶段
                    mission_state['phase'] = 'circle_scanning'
                    mission_state['circle_center'] = (target_x, target_y)
                    mission_state['circle_progress'] = 0.0
                    mission_state['accumulated_angle'] = 0.0
                    mission_state['last_angle'] = None
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
                else:
                    # 继续向航点移动
                    return np.array([drone.max_speed, math.atan2(dy, dx), 1.0])
            else:
                # 没有更多航点，返回母舰
                mission_state['phase'] = 'returning'
                return self._drone_return_to_parent_action(drone, observation)
        
        # 阶段4: 返回母艇
        elif phase == 'returning' or phase == 'mission_completed':
            return self._drone_return_to_parent_action(drone, observation)
        
        # 默认行为
        return np.array([drone.max_speed, 0.0, 1.0])
    
    def _perform_circle_scan_fsm(self, drone: WhiteDrone, mission_state: Dict, observation: AgentObservation) -> np.ndarray:
        """
        执行圆形扫描（与simple_line_defense_agent相同的逻辑）
        """
        center_x, center_y = mission_state['circle_center']
        radius = mission_state['circle_radius']
        
        # 计算当前位置相对于圆心的角度
        current_angle = math.atan2(drone.y - center_y, drone.x - center_x)
        
        # 初始化累积角度
        if mission_state['last_angle'] is None:
            mission_state['last_angle'] = current_angle
        
        # 计算这一步转过的角度
        last_angle = mission_state['last_angle']
        angle_delta = current_angle - last_angle
        
        # 处理角度跳变
        if angle_delta > math.pi:
            angle_delta -= 2 * math.pi
        elif angle_delta < -math.pi:
            angle_delta += 2 * math.pi
        
        # 累积转过的角度
        if abs(angle_delta) < math.pi / 2:
            mission_state['accumulated_angle'] += abs(angle_delta)
        
        mission_state['last_angle'] = current_angle
        mission_state['circle_progress'] = mission_state['accumulated_angle'] / (2 * math.pi)
        
        # 检查是否完成一圈扫描 (降低完成阈值，加快扫描)
        if mission_state['circle_progress'] >= 0.85:  # 从0.99降低到0.85
            mission_state['scan_completed'] = True
        
        # 继续圆形运动
        tangent_angle = current_angle + math.pi / 2
        circle_speed = drone.max_speed * 0.8  # 提高扫描速度到最大速度
        
        return np.array([circle_speed, tangent_angle, 1.0])
    
    def _process_detected_targets_at_waypoint(self, drone: WhiteDrone, mission_state: Dict, observation: AgentObservation):
        """
        处理在航点探测到的目标，记录用于后续分组
        """
        detected_targets = observation.get_detected_targets()
        waypoint_targets = []
        
        for target in detected_targets:
            waypoint_targets.append({
                'id': target.id,
                'position': (target.x, target.y),
                'speed': getattr(target, 'speed', 0.0),
                'heading': getattr(target, 'heading', 0.0),
                'waypoint_index': mission_state['current_target'],
                'detection_time': observation.current_time
            })
        
        mission_state['detected_at_waypoint'].append(waypoint_targets)
        
        # 新增：拦截点逻辑
        self._update_boat_intercept_point_after_scan(drone, mission_state, waypoint_targets, observation)
        
        # TODO: 在这里进行目标分组（你提到等下再想分组策略）
        self._group_detected_targets(waypoint_targets)
        
        # TODO: 根据分组结果调整白方无人船航线（你提到等下再想）
        self._adjust_boat_routes_based_on_groups()
    
    def _update_boat_intercept_point_after_scan(self, drone: WhiteDrone, mission_state: Dict, waypoint_targets: list, observation: AgentObservation):
        """
        每次圆形扫描后，计算并更新母舰（无人船）的拦截点
        只在DEPLOYMENT阶段执行，避免在PATROL阶段覆盖目标分配
        """
        # 只在DEPLOYMENT阶段处理拦截点计算
        if self.global_state != GlobalState.DEPLOYMENT:
            return
        
        # 只在第一次和第二次扫描后处理
        scan_index = mission_state['current_target']

        # 找到母舰
        parent_boat = None
        for boat in observation.white_boats:
            if boat.id == drone.parent_boat_id and boat.active:
                parent_boat = boat
                break
        if parent_boat is None:
            return
        # 找到x值最小的黑方无人船
        if not waypoint_targets:
            return  # 没有目标不变
        min_x_target = min(waypoint_targets, key=lambda t: t['position'][0])
        # 计算预计拦截点
        intercept_result = self._calculate_intercept_point(
            parent_boat,
            min_x_target['position'],
            min_x_target['speed'],
            min_x_target['heading']
        )
        
        # 检查拦截点是否有效
        if intercept_result is None:
            print(f"[FSM] Boat {parent_boat.id} 目标 {min_x_target['id']} 拦截点在封锁线后，放弃此目标")
            print(f"[FSM] 目标位置: {min_x_target['position']}, 速度: {min_x_target['speed']:.2f}, 航向: {min_x_target['heading']:.2f}")
            return

        intercept_point, intercept_time, boat_pos = intercept_result
        boat_id = parent_boat.id
        # 第一次扫描，强制更新
        if scan_index == 0:
            self.boat_intercept_info[boat_id] = {
                'target_id': min_x_target['id'],
                'intercept_point': intercept_point,
                'intercept_time': intercept_time,
                'scan_index': 0
            }
            # 存储拦截点到专门的数据结构中，避免与目标分配冲突
            self.boat_intercept_points[boat_id] = intercept_point
            print(f"[FSM] Boat {boat_id} 第一次扫描后拦截目标 {min_x_target['id']}，拦截点 {intercept_point}，预计拦截时间 {intercept_time:.2f}")
        # 第二次扫描，若更早则替换
        elif scan_index == 1:
            prev = self.boat_intercept_info.get(boat_id)
            if prev is None or intercept_time < prev['intercept_time']:
                self.boat_intercept_info[boat_id] = {
                    'target_id': min_x_target['id'],
                    'intercept_point': intercept_point,
                    'intercept_time': intercept_time,
                    'scan_index': 1
                }
                # 存储拦截点到专门的数据结构中，避免与目标分配冲突
                self.boat_intercept_points[boat_id] = intercept_point
                print(f"[FSM] Boat {boat_id} 第二次扫描后拦截目标 {min_x_target['id']}，拦截点 {intercept_point}，预计拦截时间 {intercept_time:.2f}（已更新）")
            else:
                print(f"[FSM] Boat {boat_id} 第二次扫描后拦截点未更新，仍为 {prev['intercept_point']}，预计拦截时间 {prev['intercept_time']:.2f}")

    # ==================== 计算工具函数 ====================
    
    def _calculate_intercept_point(self, boat, target_pos, target_speed, target_heading)-> Optional[Tuple[Tuple[float, float], float, Tuple[float, float]]]:
        """
        计算极限拦截点（考虑40000m锁定距离，不可拦截的返回None）
        boat: 白方无人船对象
        target_pos: (x, y)
        target_speed: float
        target_heading: float（弧度）
        返回：(intercept_x, intercept_y), intercept_time, (boat_x, boat_y) 或 None（如果无法拦截）
        """
        # 使用新的捕食者-猎物拦截算法
        result = compute_capture(
            X0=boat.x, Y0=boat.y,
            X1=float(target_pos[0]), Y1=float(target_pos[1]),
            ux=float(math.cos(float(target_heading))), uy=float(math.sin(float(target_heading))),
            R=40000.0,
            vp=float(boat.max_speed),
            vq=float(target_speed)
        )

        # 兼容字典式返回
        capture_ok: bool = False
        intercept_time: Optional[float] = None
        prey_pos: Optional[Tuple[float, float]] = None
        if isinstance(result, dict):
            # 强制转换类型，避免类型不兼容
            capture_raw = result.get("capture", False)
            capture_ok = bool(capture_raw)
            t_raw = result.get("t", None)
            # 某些情形 t_raw 可能为 numpy 类型，转为内置 float
            if t_raw is None:
                intercept_time = None
            else:
                try:
                    # 统一经由字符串转换，避免类型检查器报错
                    intercept_time = float(str(t_raw))
                except Exception:
                    intercept_time = None
            prey_raw: Any = result.get("prey_pos", None)
            if isinstance(prey_raw, (list, tuple)) and len(prey_raw) >= 2:
                try:
                    # 统一使用字符串路径转换，避免类型不兼容告警
                    x0 = float(str(prey_raw[0]))
                    y0 = float(str(prey_raw[1]))
                    prey_pos = (x0, y0)
                except Exception:
                    prey_pos = None
            else:
                prey_pos = None
        else:
            # 非预期返回格式，放弃
            return None

        if not capture_ok or intercept_time is None or prey_pos is None:
            return None

        try:
            t_val = float(intercept_time)
            prey_tuple = tuple(float(v) for v in prey_pos)
            if len(prey_tuple) != 2:
                return None
        except Exception:
            return None

        # 计算无人船在拦截时的位置（使用无人船当前航向）
        boat_x = float(boat.x + boat.max_speed * math.cos(boat.heading) * t_val)
        boat_y = float(boat.y + boat.max_speed * math.sin(boat.heading) * t_val)

        # 拦截点安全性：避免封锁线后
        if prey_tuple[0] < self.blockade_line_x + 3000:
            return None

        return (prey_tuple[0], prey_tuple[1]), t_val, (boat_x, boat_y)

    
    def _group_detected_targets(self, waypoint_targets: List[Dict]):
        """
        对探测到的目标进行分组
        TODO: 分组策略待实现
        """
        # 暂时空实现，等待你提供分组策略
        pass
    
    def _adjust_boat_routes_based_on_groups(self):
        """
        根据分组结果调整白方无人船航线
        TODO: 航线调整策略待实现
        """
        # 暂时空实现，等待你提供调整策略
        pass
    
    # 无人艇动作实现
    # ==================== DEPLOYMENT阶段函数 ====================
    
    def _execute_deployment_boat_action(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """
        DEPLOYMENT阶段无人船动作：向部署线移动
        
        策略：
        - 计算无人船在部署线上的目标位置
        - 以最大速度向目标位置移动
        - 不锁定任何目标（lock_id = -1）
        """
        # 计算部署线上的目标位置（根据船只索引均匀分布）
        target_pos = self._get_deployment_position(boat, observation.get_active_boats())
        
        # 计算移动方向和速度
        dx = target_pos[0] - boat.x
        dy = target_pos[1] - boat.y
        heading = math.atan2(dy, dx)
        speed = boat.max_speed  # 以最大速度移动
        
        # 不锁定任何目标
        return np.array([speed, heading, -1])

    def _is_drone_ready_on_boat(self, boat: WhiteBoat, observation: AgentObservation) -> bool:
        """
        检查无人船上是否有可发射的无人机
        
        条件：
        - 无人机停靠在该船上
        - 无人机状态为充电中（recharging）
        """
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging"):
                return True
        return False
    
    def _get_deployment_position(self, boat: WhiteBoat, active_boats: List[WhiteBoat]) -> Tuple[float, float]:
        """
        计算无人船在部署线上的目标位置
        
        部署策略：
        - 所有船只沿部署线（x=60000）均匀分布
        - 第一艘船在部署线起点，最后一艘船在部署线终点
        - 其他船只在两点之间均匀分布
        
        Args:
            boat: 目标船只
            active_boats: 活跃船只列表
            
        Returns:
            (x, y): 部署线上的目标位置
        """
        try:
            boat_index = active_boats.index(boat)
            total_boats = len(active_boats)
        except ValueError:
            # 如果船只不在活跃列表中，返回部署线中点
            mid_y = (self.deployment_line_y_start + self.deployment_line_y_end) / 2
            return (self.deployment_line_x, mid_y)
        
        # 计算部署位置
        if total_boats == 1:
            # 只有一艘船时，放在部署线中点
            target_y = (self.deployment_line_y_start + self.deployment_line_y_end) / 2
        else:
            # 多艘船时，在部署线上均匀分布
            ratio = boat_index / (total_boats - 1)  # 0到1之间的比例
            target_y = self.deployment_line_y_start + ratio * (self.deployment_line_y_end - self.deployment_line_y_start)
        
        target_x = self.deployment_line_x
        
        return (target_x, target_y)
    
    def _boat_hold_position(self, boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """保持位置"""
        lock_action = self._should_lock_target(boat, observation)
        
        # 在defensive阶段，主动发射无人机增强探测
        drone_action = 0.0
        if self.global_state == GlobalState.DEFENSIVE:
            for drone in observation.all_drones:
                if (drone.docked_on_boat_id == boat.id and 
                    drone.state == "recharging" and 
                    drone.energy >= 15.0):
                    drone_action = 1.0
                    print(f"[FSM] DEFENSIVE阶段：Boat {boat.id} 在保持位置时发射无人机 {drone.id} 增强探测")
                    break
        else:
            drone_action = self._should_launch_drone(boat, observation)
            
        # 使用lock_id = -1表示不锁定任何目标
        return np.array([0.0, boat.heading, -1])
    
    def _create_drone_patrol_path(self, observation: AgentObservation   ):
        # 2号无人机TSP路径生成 ======
                
        # 获取所有黑方目标的预测位置和ID
        points_with_ids = self._all_interceptable_black_ids(observation)
        if not points_with_ids:
            print("[FSM] 2号无人机TSP路径生成失败：没有可拦截的目标")
        elif len(points_with_ids) >= 2:
            try:
             # 提取坐标点用于TSP求解
                points = [point_info['position'] for point_info in points_with_ids]
                order, cost = solve_tsp_in_memory(points)
                    
                # 检查order是否为None
                if order is None:
                    print("[FSM] 2号无人机TSP路径生成失败：order为None")
                else:
                    # 根据TSP顺序重新排列目标ID
                    tsp_id_order = [points_with_ids[i]['target_id'] for i in order]
                    self._drone2_tsp_path = tsp_id_order
                    print(f"[FSM] 2号无人机TSP路径生成完成，目标id顺序: {tsp_id_order}, 路径长度: {cost:.2f}")
            except Exception as e:
                print(f"[FSM] 2号无人机TSP路径生成失败: {e}")
        else:
            print(f"[FSM] 2号无人机TSP路径生成失败：可拦截目标点数不足2（当前: {len(points_with_ids)}）")

    def _create_drone3_patrol_path_all_targets(self, observation: AgentObservation):
        """3号无人机专用：基于所有已检测目标（不局限于可拦截/封锁线）构建TSP顺序"""
        now = observation.current_time
        points_with_ids = []
        for unit_id, info in self.detected_targets.items():
            x, y = info['position']
            speed = info.get('speed', 0.0)
            heading = info.get('heading', 0.0)
            last_seen = info.get('last_seen', now)
            dt = now - last_seen
            pred_x = x + speed * math.cos(heading) * dt
            pred_y = y + speed * math.sin(heading) * dt
            points_with_ids.append({'target_id': unit_id, 'position': [pred_x, pred_y]})

        if len(points_with_ids) >= 2:
            try:
                points = [p['position'] for p in points_with_ids]
                order, cost = solve_tsp_in_memory(points)
                if order is not None:
                    self._drone3_tsp_path = [points_with_ids[i]['target_id'] for i in order]
                    print(f"[FSM] 3号无人机TSP路径(全目标)生成完成，目标id顺序: {self._drone3_tsp_path}, 路径长度: {cost:.2f}")
                else:
                    self._drone3_tsp_path = [p['target_id'] for p in points_with_ids]
                    print("[FSM] 3号无人机TSP路径(全目标)order为None，使用原序")
            except Exception as e:
                print(f"[FSM] 3号无人机TSP路径(全目标)生成失败: {e}")
                self._drone3_tsp_path = [p['target_id'] for p in points_with_ids]
        elif len(points_with_ids) == 1:
            self._drone3_tsp_path = [points_with_ids[0]['target_id']]
        else:
            self._drone3_tsp_path = []

    def _should_lock_target(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """判断是否应该锁定目标"""
        detected_targets = observation.get_detected_targets()
        for target in detected_targets:
            distance = boat.distance_to(target)
            if distance <= GameConfig.WHITE_BOAT_LOCK_RANGE:
                print(f"[FSM] 舰船{boat.id} 发现可锁定目标 unit_id={target.id}，距离: {distance:.0f}m，锁定范围: {GameConfig.WHITE_BOAT_LOCK_RANGE}m")
                return 1.0
        return 0.0
    
    def _should_launch_drone(self, boat: WhiteBoat, observation: AgentObservation) -> float:
        """判断是否应该发射无人机"""
        for drone in observation.all_drones:
            if (drone.docked_on_boat_id == boat.id and 
                drone.state == "recharging" and 
                drone.energy >= 15.0):
                return 1.0
        return 0.0
    
    # Removed unused _get_next_patrol_point
    
    def _find_parent_boat(self, drone: WhiteDrone, observation: AgentObservation) -> Optional[WhiteBoat]:
        """找到无人机的母舰"""
        # 首先查找无人机停靠的船只
        if drone.docked_on_boat_id is not None:
            for boat in observation.white_boats:
                if boat.id == drone.docked_on_boat_id and boat.active:
                    return boat
        
        # 如果没有停靠信息，查找parent_boat_id
        if hasattr(drone, 'parent_boat_id') and drone.parent_boat_id is not None:
            for boat in observation.white_boats:
                if boat.id == drone.parent_boat_id and boat.active:
                    return boat
        
        # 最后查找最近的活跃船只作为母舰
        active_boats = [boat for boat in observation.white_boats if boat.active]
        if active_boats:
            nearest_boat = min(active_boats, 
                             key=lambda b: math.hypot(b.x - drone.x, b.y - drone.y))
            return nearest_boat
        
        return None

    # ===== DEFENSIVE阶段：无目标巡逻点与分配 =====
    def _compute_deployment_centers(self, num_centers: int = 5) -> List[Tuple[float, float]]:
        """基于部署线生成等距巡逻中心点列表（稳定、与船顺序无关）"""
        x = float(self.deployment_line_x)
        y0 = float(self.deployment_line_y_start)
        y1 = float(self.deployment_line_y_end)
        if num_centers <= 1:
            mid_y = (y0 + y1) / 2.0
            return [(x, mid_y)]
        centers: List[Tuple[float, float]] = []
        for i in range(num_centers):
            ratio = i / float(num_centers - 1)
            yi = y0 + ratio * (y1 - y0)
            centers.append((x, yi))
        return centers

    def _has_target(self, boat: WhiteBoat) -> bool:
        """该船是否"有目标"（当前锁定目标或分配目标列表非空）"""
        # 当前锁定目标
        ct = getattr(boat, 'current_target', None)
        if ct is not None and getattr(ct, 'active', False):
            return True
        # 分配目标列表
        ids = self.boat_targets.get(boat.id, [])
        return bool(isinstance(ids, list) and len(ids) > 0)

    def _rebuild_defensive_patrol_assignments(self, observation: AgentObservation) -> None:
        """重建DEFENSIVE阶段的巡逻中心分配：
        先为有目标船预留最近中心点，再为无目标船分配最近的剩余中心。
        不影响有目标船的动作，仅用于无目标船巡逻时的中心定位。
        """
        centers = self._compute_deployment_centers(5)
        if not centers:
            self._defensive_patrol_assignments = {}
            return

        active_boats: List[WhiteBoat] = [b for b in observation.white_boats if b.active]
        # 若已有有效分配且覆盖全部活跃船，可保留；为了简洁，此处每次重建，确保随目标状态变化及时更新
        assignments: Dict[int, Tuple[float, float]] = {}
        used_indices: Set[int] = set()

        # 第一轮：有目标的船 - 预留其最近中心
        targeted_boats = [b for b in active_boats if self._has_target(b)]
        for boat in targeted_boats:
            # 选择最近未占用的中心
            best_idx = None
            best_dist = float('inf')
            for idx, (cx, cy) in enumerate(centers):
                if idx in used_indices:
                    continue
                d = math.hypot(cx - boat.x, cy - boat.y)
                if d < best_dist:
                    best_dist = d
                    best_idx = idx
            # 分配并占用
            if best_idx is not None:
                assignments[boat.id] = centers[best_idx]
                used_indices.add(best_idx)

        # 第二轮：无目标的船 - 从剩余中心选最近
        idle_boats = [b for b in active_boats if not self._has_target(b)]
        for boat in idle_boats:
            best_idx = None
            best_dist = float('inf')
            for idx, (cx, cy) in enumerate(centers):
                if idx in used_indices:
                    continue
                d = math.hypot(cx - boat.x, cy - boat.y)
                if d < best_dist:
                    best_dist = d
                    best_idx = idx
            if best_idx is not None:
                assignments[boat.id] = centers[best_idx]
                used_indices.add(best_idx)
            else:
                # 理论上不会发生（5艘船/5中心）。兜底：允许共用最近中心一次。
                # 选择全局最近中心
                cx, cy = min(centers, key=lambda c: math.hypot(c[0] - boat.x, c[1] - boat.y))
                assignments[boat.id] = (cx, cy)

        self._defensive_patrol_assignments = assignments

    def _find_nearest_active_boat(self, drone: WhiteDrone, observation: AgentObservation) -> Optional[WhiteBoat]:
        """找到最近的活跃白方无人船"""
        active_boats = [boat for boat in observation.white_boats if boat.active]
        if not active_boats:
            return None
        return min(active_boats, key=lambda b: math.hypot(b.x - drone.x, b.y - drone.y))

    def _drone_energy_minutes_to_empty(self, drone: WhiteDrone) -> float:
        """估算无人机剩余可飞时间（秒）"""
        if drone.energy <= 0 or drone.energy_consumption_rate <= 0:
            return 0.0
        # energy按百分比，rate为%/秒
        return (drone.energy / drone.energy_consumption_rate)

    def _distance(self, x1: float, y1: float, x2: float, y2: float) -> float:
        return math.hypot(x1 - x2, y1 - y2)

    def _distance_to_polygon_boundary(self, x: float, y: float) -> float:
        """计算点到战区凸多边形边界的最近距离（米）"""
        min_distance = float('inf')
        num = len(POLYGON_POINTS)
        for i in range(num):
            x1, y1 = POLYGON_POINTS[i]
            x2, y2 = POLYGON_POINTS[(i + 1) % num]
            nx, ny = point_to_line_segment_nearest_point(x, y, x1, y1, x2, y2)
            d = math.hypot(x - nx, y - ny)
            if d < min_distance:
                min_distance = d
        return min_distance

    def _clamp_point_inside_battle_area(self, x: float, y: float, offset: float = 200.0) -> Tuple[float, float]:
        """若点越界，将其投影到最近边界并向内偏移 offset 米；否则原样返回"""
        if is_in_battle_area(x, y):
            return x, y
        # 投影到最近边界
        min_distance = float('inf')
        closest = (x, y)
        num = len(POLYGON_POINTS)
        for i in range(num):
            x1, y1 = POLYGON_POINTS[i]
            x2, y2 = POLYGON_POINTS[(i + 1) % num]
            nx, ny = point_to_line_segment_nearest_point(x, y, x1, y1, x2, y2)
            d = math.hypot(x - nx, y - ny)
            if d < min_distance:
                min_distance = d
                closest = (nx, ny)
        # 向内偏移
        cx, cy = closest
        # 近似用从边界指向点的反向作为内法线方向
        dir_x, dir_y = x - cx, y - cy
        length = math.hypot(dir_x, dir_y)
        if length > 1e-6:
            dir_x, dir_y = dir_x / length, dir_y / length
        else:
            dir_x, dir_y = 0.0, 0.0
        safe_x = cx + dir_x * offset
        safe_y = cy + dir_y * offset
        return safe_x, safe_y

    def _make_safe_circle_center(self, center: Tuple[float, float], radius: float, min_margin: float = 200.0) -> Tuple[Tuple[float, float], bool]:
        """返回(安全圆心, 是否适合盘旋)。若距离边界 < 半径+min_margin，则判定不适合盘旋。"""
        cx, cy = center
        safe_cx, safe_cy = self._clamp_point_inside_battle_area(cx, cy, offset=min_margin)
        dist_to_border = self._distance_to_polygon_boundary(safe_cx, safe_cy)
        viable = dist_to_border >= (radius + min_margin)
        return (safe_cx, safe_cy), viable

    def _ensure_safe_return_or_swap(self, drone: WhiteDrone, observation: AgentObservation) -> Optional[np.ndarray]:
        """
        能量安全检查：若能量不足以飞回母舰，则返最近白艇；
        同时安排该白艇上的无人机代替当前无人机返回其母舰。
        返回值：若需立即改写为返航动作，则返回动作向量；否则返回None继续原流程。
        """
        # 估算到母舰的飞行时间
        parent_boat = self._find_parent_boat(drone, observation)
        if parent_boat is None:
            return None
        dist_to_parent = self._distance(drone.x, drone.y, parent_boat.x, parent_boat.y)
        # 采用最大速度估算
        if drone.max_speed <= 0:
            return None
        time_to_parent = dist_to_parent / drone.max_speed
        time_left = self._drone_energy_minutes_to_empty(drone)
        # 安全裕度（例如+120秒）
        safety_margin = 120.0
        if time_left < time_to_parent + safety_margin:
            # 改为返回最近的白方无人船
            nearest_boat = self._find_nearest_active_boat(drone, observation)
            if nearest_boat is None:
                return None
            # 安排最近白艇上的无人机代替当前无人机返回其母舰
            # 查找最近白艇上是否有在充电的无人机
            replacement: Optional[WhiteDrone] = None
            for d in observation.all_drones:
                if d.id != drone.id and d.docked_on_boat_id == nearest_boat.id and d.state == "recharging":
                    replacement = d
                    break
            if replacement is not None:
                # 记录换位任务：替身无人机从nearest_boat起飞，飞向原母舰位置（使用简单引导）
                self.drone_swap_tasks[replacement.id] = {
                    'type': 'fly_to_boat',
                    'target_boat_id': parent_boat.id
                }
            # 让当前无人机返回最近白艇
            dx = nearest_boat.x - drone.x
            dy = nearest_boat.y - drone.y
            heading = math.atan2(dy, dx)
            return np.array([drone.max_speed, heading, 0.3])
        return None
    
    def _drone_return_to_parent_action(self, drone: WhiteDrone, observation: AgentObservation) -> np.ndarray:
        """
        无人机返回母舰的动作（从simple_line_defense_agent复制）
        """
        # 寻找母舰
        parent_boat = None
        for boat in observation.white_boats:
            if (boat.id == drone.parent_boat_id and 
                boat.active and 
                not boat.is_frozen(observation.current_time)):
                parent_boat = boat
                break
        
        if parent_boat is None:
            # 没有找到母舰，使用最近母舰策略
            return np.array([drone.min_speed, drone.heading, 0.3])
        
        # 计算返回方向
        dx = parent_boat.x - drone.x
        dy = parent_boat.y - drone.y
        distance = math.sqrt(dx**2 + dy**2)
        
        
        heading = math.atan2(dy, dx)
        
        # 检查是否接近母舰，如果很近则切换到降落模式
        if distance < 500:  # 0.5km内准备降落
            return np.array([21, heading, 0.1])  # 降落到起飞母舰 (action < 0.25)
        else:
            return np.array([drone.max_speed, heading, 0.8])  # 飞行模式 (action >= 0.5) - 保持探测

    # Removed unused _GroupBlackTargets

    
    def _all_interceptable_black_ids(self, observation: AgentObservation):
        """
        获取所有可拦截的黑方目标的预测位置和ID
        """
        now = observation.current_time
        points_with_ids = []
        
        # 收集白方无人船当前位置用于拦截点计算
        white_boats = [boat for boat in observation.white_boats if boat.active]
        
        for unit_id, info in self.detected_targets.items():
            pred_x, pred_y = info.get('predict_pos', info['position'])
            speed = info.get('speed', 0.0)
            heading = info.get('heading', 0.0)
            
            # 检查是否有任何白方无人船可以拦截此目标
            can_intercept = False
            for boat in white_boats:
                intercept_result = self._calculate_intercept_point(
                    boat, (pred_x, pred_y), speed, heading
                )
                if intercept_result is not None:
                    can_intercept = True
                    break
            
            # 只有可以拦截的目标才加入TSP路径
            if can_intercept:
                points_with_ids.append({
                    'target_id': unit_id,  # 使用unit_id
                    'position': [pred_x, pred_y],
                    'original_position': info['position'],
                    'speed': speed,
                    'heading': heading
                })
        
        print(f"[FSM] 2号无人机TSP路径包含 {len(points_with_ids)} 个可拦截目标")
        if points_with_ids:
            min_y = min(p['position'][1] for p in points_with_ids)
            max_y = max(p['position'][1] for p in points_with_ids)
            print(f"[FSM] 目标y坐标范围: {min_y:.0f} ~ {max_y:.0f}")
            print(f"[FSM] TSP路径目标ID列表: {[p['target_id'] for p in points_with_ids]}")
        
        return points_with_ids

    def _get_visible_targets_for_drone(self, drone: WhiteDrone, observation: AgentObservation):
        """获取无人机可见的目标"""
        visible_targets = []
        black_boats = observation.black_boats
        
        for target in black_boats:
            if not target.active:
                continue
                
            # 计算距离
            distance = math.hypot(target.x - drone.x, target.y - drone.y)
            
            # 检查是否在探测范围内
            if distance <= GameConfig.WHITE_DRONE_DETECT_RANGE:
                # 检查是否在探测角度内（60度扇形）
                dx = target.x - drone.x
                dy = target.y - drone.y
                target_angle = math.atan2(dy, dx)
                
                # 计算角度差
                angle_diff = target_angle - drone.heading
                
                # 标准化角度差到[-π, π]
                while angle_diff > math.pi:
                    angle_diff -= 2 * math.pi
                while angle_diff < -math.pi:
                    angle_diff += 2 * math.pi
                
                # 检查是否在60°扇形内（±30°）
                half_sector_angle = math.radians(GameConfig.WHITE_DRONE_DETECT_ANGLE / 2)
                if abs(angle_diff) <= half_sector_angle:
                    visible_targets.append(target)
        
        return visible_targets

    def _execute_alert_scanning(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行警戒扫视 - 以最小转弯半径飞一圈并在发现目标时做出判断"""
        center_x, center_y = alert_state['scan_center']
        current_angle = alert_state['scan_angle']
        
        # 计算最小转弯半径
        min_turn_radius = 500  # 1km转弯半径，确保较大的搜索范围
        
        # 在扫视过程中检查是否发现目标
        visible_targets = self._get_visible_targets_for_drone(drone, observation)
        
        if visible_targets:
            # 选择最近的目标
            closest_target = min(visible_targets, key=lambda t: math.hypot(t.x - drone.x, t.y - drone.y))
            dist_to_boat = math.hypot(closest_target.x - parent_boat.x, closest_target.y - parent_boat.y)
            threshold = 40000.0  # 40km阈值
            
            # 检查目标是否在母舰的目标列表中
            boat_target_list = self.boat_targets.get(parent_boat.id, [])
            target_in_list = str(closest_target.id) in boat_target_list if isinstance(boat_target_list, list) else False
            
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 在警戒扫视中发现目标 {closest_target.id}")
            print(f"                距离母舰: {dist_to_boat:.0f}m, 在目标列表中: {target_in_list}")
            
            # 情况1：距离>40km 且在本舰目标列表中
            if dist_to_boat > threshold and target_in_list:
                # 先判定200km上限：超过200km则不执行持续监视，继续警戒扫视
                if dist_to_boat > 200000.0:
                    if GameConfig.debug_mode:
                        print(f"[FSM] DEFENSIVE: 目标距母舰>200km，跳过持续监视，继续警戒扫视 (target={closest_target.id}, dist={dist_to_boat:.0f}m)")
                    # 继续扫视一步（与下方继续扫视分支一致的推进逻辑）
                    angle_increment = 0.1
                    next_angle = current_angle + angle_increment
                    target_x = center_x + min_turn_radius * math.cos(next_angle)
                    target_y = center_y + min_turn_radius * math.sin(next_angle)
                    # 更新扫视角度
                    alert_state['scan_angle'] = next_angle
                    # 若完成一圈，返航充电
                    if next_angle >= 2 * math.pi:
                        alert_state['state'] = 'charging'
                        alert_state['initial_scan_done'] = True
                        if GameConfig.debug_mode:
                            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 警戒扫视完成，返航充电")
                        return self._drone_return_to_parent_action(drone, observation)
                    # 朝向下一个扫视点
                    dx = target_x - drone.x
                    dy = target_y - drone.y
                    heading = math.atan2(dy, dx)
                    return np.array([drone.max_speed * 0.8, heading, 1.0])
                # 新增能量门槛：若能量<25%，不进入持续监视，改为继续执行警戒扫视
                if drone.energy < 25.0:
                    if GameConfig.debug_mode:
                        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 能量不足(<25%)，跳过持续监视，继续警戒扫视")
                    # 继续扫视一步（与下方继续扫视分支一致的推进逻辑）
                    angle_increment = 0.1
                    next_angle = current_angle + angle_increment
                    target_x = center_x + min_turn_radius * math.cos(next_angle)
                    target_y = center_y + min_turn_radius * math.sin(next_angle)
                    # 更新扫视角度
                    alert_state['scan_angle'] = next_angle
                    # 若完成一圈，返航充电
                    if next_angle >= 2 * math.pi:
                        alert_state['state'] = 'charging'
                        alert_state['initial_scan_done'] = True
                        if GameConfig.debug_mode:
                            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 警戒扫视完成，返航充电")
                        return self._drone_return_to_parent_action(drone, observation)
                    # 朝向下一个扫视点
                    dx = target_x - drone.x
                    dy = target_y - drone.y
                    heading = math.atan2(dy, dx)
                    return np.array([drone.max_speed * 0.8, heading, 1.0])
                # 否则（能量≥25%）维持原有持续监视逻辑
                alert_state['state'] = 'monitoring'
                alert_state['target_id'] = str(closest_target.id)
                alert_state['initial_scan_done'] = True
                print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 开始监视目标 {closest_target.id} (>40km, 已分配)")
                return self._track_target_for_detection(drone, closest_target)
            
            # 情况2：距离>40km 且不在本舰目标列表中
            elif dist_to_boat > threshold and not target_in_list:
                tid_str = str(closest_target.id)
                # 若能量>=30%且未对该目标执行过一次性盘旋，则启动一次性盘旋
                if drone.energy >= 30.0 and tid_str not in self.drone_extra_circle_done:
                    # 以 predict_pos 作为圆心，若缺失则回退到当前坐标
                    predict = self.detected_targets.get(tid_str, {}).get('predict_pos', (closest_target.x, closest_target.y))
                    # 先对圆心进行边界内化与可行性评估
                    raw_center = (float(predict[0]), float(predict[1]))
                    radius = 200.0
                    safe_center, viable = self._make_safe_circle_center(raw_center, radius, min_margin=200.0)

                    alert_state['state'] = 'one_off_circle'
                    alert_state['one_off_active'] = True
                    alert_state['one_off_target_id'] = tid_str
                    alert_state['one_off_center'] = safe_center
                    alert_state['one_off_radius_m'] = radius
                    alert_state['one_off_accumulated_angle'] = 0.0
                    alert_state['one_off_last_angle'] = None
                    alert_state['one_off_mode'] = 'circle' if viable else 'visit_return'
                    alert_state['initial_scan_done'] = True

                    if GameConfig.debug_mode:
                        mode_str = 'circle' if viable else 'visit_return'
                        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 对目标 {tid_str} 启动一次性盘旋，模式={mode_str}，圆心={safe_center}")

                    # 初始一步
                    cx, cy = safe_center
                    if viable:
                        # 计算从圆心到当前机位的角度并沿切向飞行
                        angle = math.atan2(drone.y - cy, drone.x - cx)
                        tangent = angle + math.pi / 2.0
                        return np.array([drone.max_speed * 1.0, tangent, 1.0])
                    else:
                        # 访问圆心后返航
                        dx = cx - drone.x
                        dy = cy - drone.y
                        heading = math.atan2(dy, dx)
                        return np.array([drone.max_speed * 0.8, heading, 1.0])
                else:
                    # 能量不足或已尝试过，直接返回充电
                    alert_state['state'] = 'charging'
                    alert_state['initial_scan_done'] = True
                    print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 发现目标 {closest_target.id} (>40km, 非本舰目标)，返回充电（跳过一次性盘旋）")
                    return self._drone_return_to_parent_action(drone, observation)
            
            # 情况3：距离<=40km - 指引母舰发现目标（系统会自动锁定）
            else:
                alert_state['state'] = 'guiding_lock'
                alert_state['target_id'] = str(closest_target.id)
                alert_state['initial_scan_done'] = True
                # 通知母舰有近距离目标
                self.drone_close_target_alerts[parent_boat.id] = {
                    'target_id': str(closest_target.id),
                    'alert_time': observation.current_time,
                    'handled': False
                }
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 发现近距离目标 {closest_target.id} (<=40km)，指引母舰发现目标")
                return self._guide_boat_to_target(drone, closest_target, parent_boat)
        
        # 继续扫视 - 以最小转弯半径飞一圈
        angle_increment = 0.1  # 角度增量，控制扫视精度
        next_angle = current_angle + angle_increment
        
        # 计算圆周上的下一个点
        target_x = center_x + min_turn_radius * math.cos(next_angle)
        target_y = center_y + min_turn_radius * math.sin(next_angle)
        
        # 更新扫视角度
        alert_state['scan_angle'] = next_angle
        
        # 如果完成一圈，返回充电
        if next_angle >= 2 * math.pi:
            alert_state['state'] = 'charging'
            alert_state['initial_scan_done'] = True
            if GameConfig.debug_mode:
                print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 警戒扫视完成，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 朝向下一个扫视点
        dx = target_x - drone.x
        dy = target_y - drone.y
        heading = math.atan2(dy, dx)
        
        return np.array([drone.max_speed * 0.8, heading, 1.0])  # 稍慢的速度进行扫视

    def _execute_target_monitoring(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行目标监视 - 持续监视直到目标击沉（无人机只负责探测跟踪）"""
        target_id = alert_state['target_id']
        target = self._find_target_by_id(target_id, observation)
        
        if not target or not target.active:
            # 目标消失或被击沉，从母舰的目标列表中移除并转向下一个目标
            self._handle_target_destroyed_notification(target_id, parent_boat.id, observation.current_time)
            alert_state['state'] = 'charging'
            alert_state['target_id'] = None
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 监视的目标 {target_id} 已被击沉，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 继续监视目标直到击沉（无人机只负责跟踪探测，不攻击）
        if GameConfig.debug_mode:
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 持续跟踪监视目标 {target_id}")
        return self._track_target_for_detection(drone, target)

    def _execute_guiding_lock(self, drone: WhiteDrone, alert_state: dict, parent_boat: WhiteBoat, observation: AgentObservation) -> np.ndarray:
        """执行指引母舰发现目标（系统会自动锁定40km内的目标）"""
        target_id = alert_state['target_id']
        target = self._find_target_by_id(target_id, observation)
        
        if not target or not target.active:
            # 目标消失，结束指引
            alert_state['state'] = 'charging'
            alert_state['target_id'] = None
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 指引的目标 {target_id} 已消失，返航充电")
            return self._drone_return_to_parent_action(drone, observation)
        
        # 检查目标是否已经进入母舰40km锁定范围（系统会自动锁定）
        dist_to_boat = math.hypot(target.x - parent_boat.x, target.y - parent_boat.y)
        if dist_to_boat <= 40000:  # 40km内
            # 检查母舰是否已经锁定了目标
            if parent_boat.current_target and str(parent_boat.current_target.id) == target_id:
                if parent_boat.is_locking:
                    # 母舰已锁定，无人机返航
                    alert_state['state'] = 'charging'
                    alert_state['target_id'] = None
                    # 标记近距离目标警报为已处理
                    if parent_boat.id in self.drone_close_target_alerts:
                        self.drone_close_target_alerts[parent_boat.id]['handled'] = True
                    print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 母舰已锁定目标 {target_id}，返航充电")
                    return self._drone_return_to_parent_action(drone, observation)
        
        # 继续指引母舰发现目标
        return self._guide_boat_to_target(drone, target, parent_boat)

    def _execute_one_off_circle(self, drone: WhiteDrone, alert_state: dict, observation: AgentObservation) -> np.ndarray:
        """对非本舰目标在预测点执行一次性单圈盘旋，完成后返航充电"""
        # 读取参数
        center = alert_state.get('one_off_center')
        radius = float(alert_state.get('one_off_radius_m', 200.0))
        mode = alert_state.get('one_off_mode', 'circle')
        if center is None:
            # 安全兜底：若缺少中心，直接返航
            alert_state['state'] = 'charging'
            return self._drone_return_to_parent_action(drone, observation)
        cx, cy = center

        # 若为“到点即返航”模式（边界过近或圆心越界时触发）
        if mode == 'visit_return':
            dist = math.hypot(cx - drone.x, cy - drone.y)
            # 到达判定：距离小于 max(500m, 半径的一半)
            if dist <= max(500.0, radius * 0.5):
                # 清理并返航
                alert_state['state'] = 'charging'
                alert_state['one_off_active'] = False
                alert_state['one_off_target_id'] = None
                alert_state['one_off_center'] = None
                alert_state['one_off_last_angle'] = None
                alert_state['one_off_accumulated_angle'] = 0.0
                if GameConfig.debug_mode:
                    print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 一次性盘旋改为到点返航，已到达中心，返航充电")
                return self._drone_return_to_parent_action(drone, observation)
            # 向圆心飞行
            dx = cx - drone.x
            dy = cy - drone.y
            heading = math.atan2(dy, dx)
            return np.array([drone.max_speed * 0.8, heading, 1.0])

        # 在盘旋过程中记录是否看见指定目标（用于验证盘旋）
        tid_watch = alert_state.get('one_off_target_id')
        if isinstance(tid_watch, str) and tid_watch:
            visible = self._get_visible_targets_for_drone(drone, observation)
            if any(str(t.id) == tid_watch for t in visible):
                alert_state['one_off_seen_target'] = True

        # 当前位置相对圆心的角度
        current_angle = math.atan2(drone.y - cy, drone.x - cx)
        last_angle = alert_state.get('one_off_last_angle')
        if last_angle is None:
            alert_state['one_off_last_angle'] = current_angle
            last_angle = current_angle
        delta = current_angle - last_angle
        # 角度跳变归一
        if delta > math.pi:
            delta -= 2 * math.pi
        elif delta < -math.pi:
            delta += 2 * math.pi
        # 累计弧度（忽略过大跃迁）
        if abs(delta) < math.pi / 2:
            alert_state['one_off_accumulated_angle'] = alert_state.get('one_off_accumulated_angle', 0.0) + abs(delta)
        alert_state['one_off_last_angle'] = current_angle

        # 完成判定：严格一圈
        if alert_state.get('one_off_accumulated_angle', 0.0) >= 2 * math.pi * 0.85:
            tid = alert_state.get('one_off_target_id')
            if isinstance(tid, str) and tid:
                self.drone_extra_circle_done.add(tid)
            # 若是验证盘旋且整圈未见到目标 → 删除预测与分配（仅发起船）
            if alert_state.get('one_off_validation') and not alert_state.get('one_off_seen_target'):
                # 从detected_targets中删除
                if isinstance(tid, str) and tid in self.detected_targets:
                    del self.detected_targets[tid]
                    print(f"[FSM] DEFENSIVE: 验证盘旋未见目标 {tid}，已从detected_targets删除")
                # 从发起船的目标列表中删除
                parent_boat_id = alert_state.get('one_off_parent_boat_id')
                if isinstance(parent_boat_id, int) and parent_boat_id in self.boat_targets:
                    lst = self.boat_targets.get(parent_boat_id, [])
                    if isinstance(lst, list) and tid in lst:
                        lst.remove(tid)
                        print(f"[FSM] DEFENSIVE: 验证盘旋未见目标 {tid}，已从船只 {parent_boat_id} 的目标列表删除")
            # 清理并返航
            alert_state['state'] = 'charging'
            alert_state['one_off_active'] = False
            alert_state['one_off_target_id'] = None
            alert_state['one_off_center'] = None
            alert_state['one_off_last_angle'] = None
            alert_state['one_off_accumulated_angle'] = 0.0
            print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 一次性盘旋完成，返航充电")
            return self._drone_return_to_parent_action(drone, observation)

        # 继续沿切向飞行（速度系数=1）
        tangent_angle = current_angle + math.pi / 2.0
        target_x = cx + radius * math.cos(tangent_angle)
        target_y = cy + radius * math.sin(tangent_angle)
        dx = target_x - drone.x
        dy = target_y - drone.y
        heading = math.atan2(dy, dx)
        return np.array([drone.max_speed * 0.8, heading, 1.0])
                

    def _guide_boat_to_target(self, drone: WhiteDrone, target: BlackBoat, parent_boat: WhiteBoat) -> np.ndarray:
        """指引母舰发现目标（无人机只负责探测指引，系统会自动锁定）"""
        # 无人机在目标附近保持探测，让母舰能够发现目标
        # 计算一个跟随目标的位置（保持探测距离）
        follow_distance = 2000  # 跟随距离2km，确保母舰能探测到
        
        # 计算相对于目标的位置（在目标侧方，便于母舰探测）
        target_heading = target.heading
        # 在目标侧方保持位置，既能探测目标又能被母舰探测到
        follow_x = target.x + follow_distance * math.cos(target_heading + math.pi/2)
        follow_y = target.y + follow_distance * math.sin(target_heading + math.pi/2)
        
        # 朝向跟随位置
        dx = follow_x - drone.x
        dy = follow_y - drone.y
        heading = math.atan2(dy, dx)
        
        print(f"[FSM] DEFENSIVE: 无人机 {drone.id} 指引母舰发现目标 {target.id}")
        return np.array([drone.max_speed * 0.8, heading, 1.0])

    def _handle_target_destroyed_notification(self, target_id: str, responsible_boat_id: int, destruction_time: float):
        """处理目标被击沉的通知"""
        # [FIX] 如果目标ID已在已销毁集合中，则直接返回以避免重复处理
        if str(target_id) in self.destroyed_target_ids:
            return

        # [FIX] 将目标ID添加到已销毁集合中，确保只处理一次
        self.destroyed_target_ids.add(str(target_id))
        
        # 记录击沉通知
        self.target_destruction_notifications.append((target_id, responsible_boat_id, destruction_time))
        
        # 从所有船的目标列表中移除该目标
        for boat_id in list(self.boat_targets.keys()):
            boat_target_list = self.boat_targets[boat_id]
            if isinstance(boat_target_list, list) and str(target_id) in boat_target_list:
                boat_target_list.remove(str(target_id))
                print(f"[FSM] DEFENSIVE: 从船只 {boat_id} 的目标列表中移除已击沉目标 {target_id}")
                
                # 如果这是负责击沉目标的船只，通知其他可能分配了该目标的船只
                if boat_id == responsible_boat_id:
                    self._notify_other_boats_target_destroyed(target_id, boat_id)
        
        # 从检测目标中移除
        if str(target_id) in self.detected_targets:
            del self.detected_targets[str(target_id)]
            
        print(f"[FSM] DEFENSIVE: 目标 {target_id} 被击沉，负责船只: {responsible_boat_id}")

    def _maybe_launch_validation_circle(self, boat: WhiteBoat, target_id: str, observation: AgentObservation) -> bool:
        """若条件满足，发起一次性'验证盘旋'：到目标预测点单圈，未见则删除并换下一个。
        返回是否成功发起。仅在DEFENSIVE、队首目标、无人机可用、且该tid未尝试过时触发。
        """
        if self.global_state != GlobalState.DEFENSIVE:
            return False
        tid = str(target_id)
        if tid in self.drone_extra_circle_done:
            return False
        # 必须是该船队首目标
        bt = self.boat_targets.get(boat.id, [])
        if not (isinstance(bt, list) and bt and str(bt[0]) == tid):
            return False
        # 必须存在预测信息
        info = self.detected_targets.get(tid)
        if not isinstance(info, dict):
            return False
        px, py = info.get('predict_pos', info.get('position', (None, None)))
        if not (isinstance(px, (int, float)) and isinstance(py, (int, float))):
            return False
        # 找可用无人机（本舰，充电中，能量≥30%）
        candidate = None
        for d in observation.all_drones:
            if d.docked_on_boat_id == boat.id and d.state == 'recharging' and getattr(d, 'energy', 0.0) >= 30.0:
                candidate = d
                break
        if candidate is None:
            return False
        # 初始化one_off_circle验证状态
        st = self.drone_alert_state.setdefault(candidate.id, {
            'state': 'charging',
            'last_alert_time': 0,
            'target_id': None,
            'scan_center': None,
            'scan_angle': 0,
            'initial_scan_done': True
        })
        # 进入一次性“验证盘旋”前，矫正中心并评估可行性
        raw_center = (float(px), float(py))
        radius = 200.0
        safe_center, viable = self._make_safe_circle_center(raw_center, radius, min_margin=200.0)
        st['state'] = 'one_off_circle'
        st['one_off_active'] = True
        st['one_off_target_id'] = tid
        st['one_off_center'] = safe_center
        st['one_off_radius_m'] = radius
        st['one_off_accumulated_angle'] = 0.0
        st['one_off_last_angle'] = None
        st['one_off_validation'] = True
        st['one_off_seen_target'] = False
        st['one_off_parent_boat_id'] = int(boat.id)
        st['one_off_mode'] = 'circle' if viable else 'visit_return'
        if GameConfig.debug_mode:
            mode_str = 'circle' if viable else 'visit_return'
            scx, scy = safe_center
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 发起验证盘旋 无人机{candidate.id} 目标{tid} 模式={mode_str} 圆心({scx:.0f},{scy:.0f})")
        return True

    def _notify_other_boats_target_destroyed(self, target_id: str, responsible_boat_id: int):
        """通知其他船只目标已被击沉"""
        for boat_id in self.boat_targets:
            if boat_id != responsible_boat_id:
                boat_target_list = self.boat_targets[boat_id]
                if isinstance(boat_target_list, list) and str(target_id) in boat_target_list:
                    boat_target_list.remove(str(target_id))
                    print(f"[FSM] DEFENSIVE: 通知船只 {boat_id} 移除已被其他船只击沉的目标 {target_id}")

    def _get_next_target_for_boat(self, boat: WhiteBoat) -> Optional[str]:
        """为无人船获取下一个目标"""
        boat_target_list = self.boat_targets.get(boat.id, [])
        
        if isinstance(boat_target_list, list) and boat_target_list:
            # 返回列表中的第一个目标
            next_target_id = boat_target_list[0]
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 转向下一个目标 {next_target_id}")
            return next_target_id
        else:
            # 目标列表为空，在原地警戒
            print(f"[FSM] DEFENSIVE: 船只 {boat.id} 目标列表已清空，保持警戒")
            return None


def create_fsm_defense_agent() -> FSMDefenseAgent:
    """工厂函数，创建FSM防御策略智能体"""
    return FSMDefenseAgent()
